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ABSTRACT 


The  path  planning  algorithm  in  Yamabico  is  based  on  a  variation  of  Dijkstra’s 

algorithm  which  has  time  complexity  of  0  (n^) .  This  algorithm  works  well  in  a  dynamic 
environment,  but  a  faster  algorithm,  called  the  All-Pairs  Minimum  Cost  Paths  algorithm, 
works  even  faster,  0(1) ,  in  the  case  of  a  static  environment. 

The  computational  complexity  of  the  All-Pairs  algorithm  is  0  {n) ,  but  if  we  know 
all  pairs  in  advance,  that  is,  the  environment  is  static,  we  can  preprocess  them  in  advance, 
and  use  table  lookup  instead  of  Dijkstra’s  algorithm.  Thus,  we  implemented  a  table  lookup 
version  for  the  static  case,  and  kept  Dijkstra’s  algorithm  for  the  dynamic  case.  This  results 
in  both  speed  and  flexibility. 

This  thesis  also  investigated  the  Linear  Fitting  Algorithm  for  Sonar  testing.  Range 
and  angle  data,  from  sonar,  was  fit  to  a  straight  line,  giving  resolution  of  1  to  2.5  cm  when 
the  robot  is  within  100  to  150  cm  of  the  line. 
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I.  INTRODUCTION 


A.  BACKGROUND 

One  of  the  ultimate  goals  in  robotics  is  to  develop  autonomous  robots.  Specifically, 
robots  capable  of  successfully  completing  a  task  when  provided  instmctions  on  what  to  do 
but  not  how  to  do  it.  Potential  uses  for  sensor  based  robotics  in  stmctured  environments 
abound.  Increasingly  capable  autonomous  mobile  robot  platforms  are  being  developed  to 
handle  a  myriad  of  hazardous  duty  assignments.  While  manufacturing  tasks  dominate  the 
area  of  robotic  applications,  useful  advances  have  been  made  in  the  areas  of  waste 
management,  space  exploration,  undersea  work,  assistance  for  the  disabled  and  medical 
surgery.  Other  examples  include  factory  delivery  systems,  toxic  waste  disposal,  pipeline 
inspections,  etc.  Several  U.S.  government-sponsored  efforts  are  underway  for  building 
systems  for  military  applications  such  as  combating,  handling  ammunition,  transporting 
material,  underwater  search  and  inspection  operations,  and  other  dangerous  tasks  currently 
performed  by  humans.  Therefore,  studying  intelligent  sensor-based  systems  is  one  of  the 
major  areas  in  the  field  of  robotics  today.  In  order  to  achieve  a  mission,  sensors  are  used  for 
data  acquisition,  a  strategy  based  on  the  environment  and  state  of  the  robot  is  chosen,  and 
sensing  is  then  integrated  into  the  planning  process.  Hence,  the  global  path  planning 
system,  local  motion  planning  strategies,  and  sensing  system  are  integrated  into  one 
coherent  system. 

B.  OVERVIEW 

1.  Sonar 

Sonars  are  used  to  determine  location  and  recognize  obstacles  to  be  avoided.  The 
sonar  data  are  the  inputs  to  several  functions  of  a  high  level  language  called  MML  (model 
based  mobile  robot  language),  which  is  the  driving  force  behind  the  robot  Yamabico. 

Although  Yamabico  may  have  precise  knowledge  of  its  location  in  a  given 
environment,  it  is  only  capable  of  detecting  the  presence  of  unexpected  obstacles  in  its  path 
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using  its  12  sonars.  Yamabico  can  move  at  a  speed  up  to  65  centimeters  per  second  in  a 
translational  motion,  forward  or  backwards. 

In  motion  planning  we  attempt  to  apply  the  sonar  system  in  parallel,  so  that 
combining  the  two  techniques  leads  to  a  better  “understanding”  of  the  world  and  thus 
reduces  the  robot’s  positional  errors.  Thus,  sonar  accuracy  is  very  important  as  it  affects  the 
efficiency  of  motion  planning  and  obstacle  avoidance. 

2.  Global  Motion  Planning 

Many  of  the  robot’s  tasks  require  motion.  Deciding  how  to  move  from  one  location 
to  another  is  known  as  the  motion  planning  problem.  In  motion  planning,  not  only  is 
position  important,  but  the  orientation  and  curvature  of  the  vehicle  are  important  as  it 
follows  the  path.  These  elements  are  represented  in  the  configuration  tuple  (j:,  y,  0,  k)  . 
Hence,  the  motion  planning  problem  can  be  stated:  How  can  a  robot  decide  what  motions 
to  perform  in  order  to  move  from  one  configuration  to  another? 

Motion  planning  rather  than  path  planning  is  used,  because  vehicles  considered 
here  are  not  points,  but  rigid  bodies.  In  path  planning,  the  result  is  a  series  of  positions 
which  must  be  followed  by  the  vehicle.  For  an  autonomous  vehicle,  planning  motions 
which  avoid  known  and  unknown  objects  in  its  environment  is  the  most  fundamental 
functionality.  For  example,  the  mission  of  mine  detection  and  clearance  could  not  be 
successful  unless  the  motion  planning  is  solved.  How  should  the  data  be  structured  to 
capture  the  topology  of  the  operating  environment,  enable  efficient  processing  of  data, 
verify  the  robot  location,  and  modify  (if  necessary)  the  global  path  plan?  Consistency  in 
both  global  and  local  plans  can  be  achieved  through  constant  collection  and  management 
of  sensor  data  and  using  the  data  to  guide  the  robot  actions.  All  robot  actions  must  be  based 
on  the  status  of  the  robot  in  conjunction  with  its  environment. 
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n.  PROBLEM  STATEMENTS 


A.  SONAR  CHARACTERISTICS  MEASUREMENT 

The  Sonar  Testing  problem  is  to  examine  the  precision  of  the  sonar  interpretation 
data,  obtained  by  the  current  Linear  Fitting  Algorithm.  Theoretical  study,  extensive 
simulations  and  testing  are  required  to  make  this  determination.  The  methodology  of  this 
thesis  is  to  program  the  robot’s  motion  in  an  known  world,  (Figure  1.),  and  then  estimate 
how  well  it  understands  this  world.  The  experiments  concentrate  primarily  on  the  left  and 
right  sonars. 


Wall 

Doorway  L 

i  I 

Sonar  #7 

- -  O 

Sonar  #5 

1  r 

Wall  i— 


Figure  1.  Robot’s  Motion  in  a  Known  World 

Motion  planning  methods  and  algorithms  need  accurate  data  from  the  robot’s  sonar 
system.  This  enables  verification  that  the  robot  is  at  the  calculated  position  relative  to  the 
world’s  objects  (polygons).  If  there  is  a  difference  between  the  actual  and  computed 
positions  the  software  calculates  the  positional  error. 
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The  Linear  Fitting  Algorithm  makes  significant  contributions  toward  solving  the 
robot  motion  problem,  (see  Chapter  V).  Applying  the  theory  in  robotics,  the  sonar’s  returns 
are  inputs  to  the  algorithm,  and  the  result  is  an  interpretation  of  the  sensed  world  compared 
to  the  known  world.  How  well  this  combination  of  hardware  and  software  resolves  the 
disparity  between  the  sensed  world  and  known  world  is  the  criterion  for  the  optimal  sonar 
configuration. 

B.  GLOBAL  MOTION  PLANNING 

The  Global  Motion  Planning  problem  is,  given  the  robot’s  operating  environment 
(world),  to  finding  the  optimal  path  class. 

The  framework  of  the  motion  planning  problem  is  set  in  a  two  dimensional, 
rectilinear  world.  The  objective  is  to  quickly  and  safely  navigate  an  autonomous  vehicle 
through  free  space  from  an  initial  configuration  to  a  goal  configuration  using  smooth 
motions  (Figure  2.).  A  two  dimensional,  rectilinear  world  is  used,  since  that  type  of  world 


Figure  2.  Motion  Planning  Problem 


closely  models  the  interior  of  most  office  buildings.  A  rectilinear  world  is  one  in  which  all 
edges  are  parallel  to  either  the  global  coordinate  frame  x-axis  or  y-axis.  Furthermore,  all 
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obstacles  are  rectangles.  A  configuration  (x,  y,  9,  k)  is  a  tuple  which  describes  the  two 
dimensional  position,  the  orientation  and  the  curvature  of  the  vehicle.  Smooth  motions  are 
those  trajectories  which  possess  continuous  tangents  and  curvatures.  Free  space  is  that  part 
of  the  world  in  which  no  obstacle  is  located. 

The  following  assumptions  are  used  throughout  this  thesis: 

•  The  vehicle  and  all  objects  in  the  robot’s  environment  are  rigid  bodies. 

•  The  robot  has  complete  knowledge  of  the  environment  in  which  it  is  operating. 
However,  the  use  of  external  references  to  guide  its  motion  other  than  the  physical 
characteristics  of  the  walls  will  not  be  used. 

•  The  world  is  modeled  as  a  planar  rectilinear  world  with  obstacles. 

•  The  obstacles  do  not  intersect  or  touch  each  other. 

•  Walls  in  the  robot’s  environment  are  always  rectilinear,  as  are  found  in  most  office 
buildings. 

•  All  obstacles’  faces  are  perpendicular  to  the  plane  in  which  the  robot  moves.  This 
assumption  is  required  to  assure  a  good  sensor  return  from  all  objects. 

•  Although  the  robot  will  be  operating  in  a  three  dimensional  environment,  it  is 
assumed  that  the  model  reflects  the  projection  of  the  obstacles  onto  the  plane  of  the 
floor  on  which  the  robot  moves. 

•  All  obstacles  in  the  environment  are  immobile. 

•  Finally,  it  is  assumed  that  the  robot’s  free  space  is  bounded  by  an  inverted 
rectilinear  polygon. 
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m.  SONAR  SYSTEM 


A.  HARDWARE  SYSTEM 

Yamabico’s  sonar  hardware  is  extremely  efficient  because  a  dedicated  sonar  board 
with  a  microprocessor  controls  the  sonar  sensors  [LOC94].  Yamabico’s  main  central 
processing  unit  is  interrupted  only  when  data  becomes  available  from  the  sonar  array.  The 
sonar  system  provides  user  interface  functions  that  control  Yamabico’s  array  of  sonar  range 
finders.  At  any  point  within  a  user’s  program,  any  of  the  12  sonars  may  be  enabled  or 
disabled.  This  allows  the  user  to  operate  a  given  sonar  only  when  necessary  for  a  particular 
application.  When  needed,  the  sonar  system  returns  the  latest  reading  of  a  specified  sonar 
out  of  the  twelve.  This  system  design  is  far  better  than  the  primitive  one  in  which  a  user 
must  wait  30  milliseconds  after  he/she  issues  a  command.  A  user’s  program  can  also  be 
forced  to  “busy  wait”  until  some  sonar-based  condition  is  satisfied.  This  feature  is 
particularly  valuable  for  obstacle  avoidance.  For  example,  a  user’s  program  could  be 
written  to  wait  until  the  forward  looking  sonar’s  range  is  less  than  distance  d,  then  stop.  A 
block  diagram  of  the  sonar  system  is  provided  in  Figure  3. 


11  0  3 


Figure  3.  Yamabico  Sonars 
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Figure  4.  shows  the  current  hardware  configuration  of  Yamabico. 


1.  Sonar  Grouping 

In  order  to  reduce  sampling  time  the  sonars  are  operated  in  logical  groups  of  four. 
The  sonars  of  a  logical  group  are  all  pulsed  simultaneously  and  thus  the  sampling  time  is 
reduced  by  a  factor  of  four  as  compared  to  individual  firing  of  the  sonars.  The  sonars  of 
each  logical  group  are  oriented  in  such  a  way  as  to: 

-  prevent  mutual  interference 

-  provide  a  “look”  in  all  four  directions  from  each  group 

-  present  a  similar  aspect  from  each  sonar  during  a  rotational  scan 
Thus,  logical  group  0  consists  of  sonars  0,  2,  5  and  7  (see  Figure  3.),  group  1 

consists  of  sonars  1, 3, 4  and  6;  group  2  consists  of  sonars  8, 9,  10  and  11;  (see  page  9)  and 
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group  3  is  a  “virtual”  group  which  consists  of  four  permanent  test  values.  The  sonars  of  a 
group  are  symmetric  about  the  robot’s  axis  of  rotation. 


TABLE  1:  Sonar  Mnemonics 


Mnenonic 

Sonar 

Group 

SOOO 

0 

0 

S030 

3 

1 

S060 

10 

2 

S090 

7 

0 

S120 

6 

1 

S150 

9 

2 

S180 

2 

0 

S210 

1 

1 

S240 

8 

2 

S270 

8 

2 

S300 

4 

1 

S330 

11 

2 

In  addition  to  being  logically  grouped,  the  sonars  are  also  physically  grouped  (see 
Figure  4.).  The  physical  grouping  of  the  sonars  is  made  to  distribute  the  electrical  load  over 
the  driver  boards  evenly  and  thus  minimize  any  electrical  transients  associated  with 
operation  of  the  sonar.  The  physical  grouping  connects  sonars  0,  2,  8  and  1 1  to  driver/ 
amplifier  board  1 ;  sonars  4, 5, 6  and  7  to  board  2;  and  sonars  1, 3, 9  and  10  to  board  3.  The 
reader  will  note  that  pairs  of  sonars  from  logical  groups  are  assigned  to  physical  groups,  for 
example,  sonars  0  and  2  from  logical  group  0  are  assigned  to  physical  group  (driver/ 
amplifier  board)  1. 
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Initial  design  of  the  control  circuitry  was  based  on  two  primary  parameters 
[BYR94]:  (1)  a  desired  maximum  range  of  400  centimeter  and  (2)  a  pulse  width  of  1 
millisecond.  Assuming  a  speed  of  sound  in  air,  at  sea  level,  of  340  meters/second  we  may 
calculate  a  round-trip  time: 

round  trip  time  =  — x2  =  23.53  msec  (Eq.  1) 

^  34000  cm/sec 

This  round  trip  time  is  the  period  during  which  a  valid  echo  may  be  received  and  is 
referred  to  as  the  receive  gate.  This  interval  is  rounded  up  to  24  milliseconds  and  is  derived 
by  division  of  the  sonar  system’s  2  MHz  clock  to  ensure  that  the  receiver  is  not  falsely 
triggered  by  a  direct  path  reception  from  it’s  adjacent  transmitter,  we  opt  to  disable  the 
receiver  until  the  transmit  pulse  is  complete.  This  will  have  the  disadvantage  of  setting  a 
minimum  range  equal  to  half  the  distance  sound  would  travel  in  the  time  of  a  transmit  pulse. 

minimum  range  =  34000  cm./sec.  x  1  msec,  x  0.5  =  17  cm.  (Eq.  2) 

This  minimiiTn  range  lies  approximately  9  centimeters  outside  the  periphery  of  the 
robot.  In  order  to  allow  the  measurement  of  objects  up  to  the  periphery  of  the  robot,  the 
pulse  width  was  decreased  to  0.5  milliseconds  thus  reducing  the  minimum  range  to  8.5 
centimeters. 

In  actual  practice,  the  minimum  range  is  set  by  firmware  to  9.6  centimeters,  the 
additional  distance  being  due  to  time  allotted  for  switching  and  settling  in  the  circuitry. 

All  sonars  of  a  logical  group  are  pulsed  simultaneously.  Which  groups  are  fired  is 
determined  by  the  value  of  the  corresponding  bit  in  the  command  register  of  the  sonar 
control  board,  which  in  turn  is  set  by  the  user  with  an  MML  function  (Figure  4.).  Hence,  if 
bit  2  is  set  to  1  then  group  2  sonars  will  be  pulsed.  If  more  than  one  group  is  selected  to  be 
pulsed,  the  sonar  control  board  will  pulse  the  first  group  on  the  list,  and  when  the  data  from 
that  pulse  has  been  read  from  the  fourth  data  register  the  sonar  control  board  will  proceed 
to  the  next  group  and  pulse  it,  and  so  on  in  round  robin  fashion.  Groups  with  their  control 
bit  set  to  0  will  not  be  pulsed.  The  sampling  rate  can  thus  be  as  high  as  41  Hz  with  only  one 
group  enabled  (based  on  a  24  millisecond  read  gate  as  determined  in  Equation  3.2)  and  will 
be  halved  for  each  additional  group  enabled.  At  a  nominal  robot  speed  of  30  centimeters 
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per  second,  this  sampling  rate  could  provide  an  updated  range  within  0.75  centimeter  of 
travel,  exceeding  our  desired  positional  accuracy  of  1  centimeter.  Of  course,  real 
performance  will  be  affected  by  any  delay  in  reading  the  data  registers  due  to  other 
demands  on  the  central  processor  (processing  the  sonar  data,  controlling  motion,  etc.). 

2.  Range  Finding 

There  are  four  16  bit  data  registers  on  the  sonar  control  board,  one  for  each  of  the 
four  sonars  in  a  logical  group.  When  the  transmit  pulse  is  sent  to  the  driver/amplifier  boards 
a  counter  is  started  which  increments  each  of  the  data  registers  every  6  microseconds.  This 
time  period  is  equivalent  to  a  range  of  1 .02  millimeter: 

range  =  340000  min/sec  x  6  microsec  x  0.5  =  1.02  mm  (Eq.  3) 

The  incrementing  of  a  particular  data  register  continues  until  an  echo  is  received  or 
the  range  gate  times  out.  The  first  12  bits  of  the  data  register  are  allotted  for  range 
accumulation,  thus  allowing  for  a  maximum  range  of  4.177  meters  (4095  x  1.02  mm.).  If 
the  range  gate  should  time  out  before  an  echo  is  received,  the  high  bit  of  the  over  ranged 
sonar’s  data  register  is  set  to  1 .  This  is  the  “over  range”  bit  and  is  used  to  signal  the  ensuing 
software  that  no  echo  was  received.  Bits  12,  13  and  14  of  the  data  registers  are  not  used. 
When  the  ranging  cycle  is  complete,  the  appropriate  group  number  is  written  into  bits  4  and 
5  of  the  status  register  and  the  “ready”  bit,  bit  7  of  the  status  register,  is  set  to  1.  The  ready 
bit  is  used  as  a  flag  when  operating  in  the  polled  mode;  i.e.  without  interrupts. 

3.  Interrupt  Control 

The  sonar  control  board  is  actually  a  daughtercard  which  rides  on  a  VME  bus 
mothercard.  The  mothercard  carries  address  decoders,  bus  drivers  and  interrupt  control 
circuitry  in  the  Bus  Interface  Module  (BIM). 

When  the  sonar  has  completed  a  ranging  cycle  an  interrupt  request  is  provided  to 
the  BIM.  The  BIM’s  control  register  holds  information  which  determines  whether  an 
interrupt  is  to  be  generated  or  not,  and  if  so  which  interrupt  level  is  to  be  generated. 
Presuming  an  interrupt  is  generated,  when  the  correct  acknowledgment  returns  on  the 
address  lines  the  BIM’s  vector  register  provides  the  vector  table  entry  where  the  central 
processor  may  find  the  vector  to  the  interrupt  handler.  The  correct  interrupt  level,  the 
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interrupt  enable  bit  and  interrupt  vector  are  loaded  to  the  BIM  during  software 
initialization. 

4.  Data  Transfer 

Each  of  the  data  registers  is  individually  addressed  on  the  VME  bus  by  a  VME  short 
address,  as  is  the  status  register.  Transferral  of  the  data  is  extremely  straightforward.  The 
interrupt  handler  simply  reads  the  correct  register,  masks  out  the  unwanted  bits  and  writes 
the  data  to  the  stack.  When  the  last  data  register  is  read,  the  sonar  system  resets  the  data 
registers  and  commences  a  ranging  cycle  on  the  next  sonar  group  in  it’s  round  robin.  The 
system  will  continue  to  operate  autonomously  until  all  the  sonars  are  disabled. 

B.  BASIC  SONAR  FUNCTIONS 

1.  Distance 

There  are  two  functions  available  to  return  sonar  values.  One  function,  sonarQ  will 
return  the  range  from  the  sonar  to  the  object  it  is  getting  the  return  from.  If  there  is  no  return, 
then  a  value  of  infinity  is  assigned,  and  for  Yamabico  this  value  is  999999.  The  infinity 
value  is  used  for  trouble  shooting  purposes,  to  detect  whether  or  not  there  are  instances  of 
no  return  from  objects  at  a  distance  of  less  than  4  meters.  The  second  range  function 
available  is  global(),  and  this  will  return  the  x,y  coordinates  of  where  the  return  was 
detected  in  the  world  that  the  vehicle  is  in.  This  is  useful  in  the  vehicle  making  a  map  of  its 
world  with  obstacles  in  it. 

2.  Global  Position  Calculations 

By  utilizing  the  compose  function  seen  in  Figure  5.,  we  can  determine  the  actual 
point  in  a  2D  coordinate  system.  Let  the  following  equations  represent  q;  an  q2, 

9i  =  (Eq.  4) 


^2  -  (*2-  >2’  ®2) 


T 


(Eq.  5) 
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The  composition  of  these  transformations  is  defined  as 


% 


''x^  +*2cos0j  -  j'jSinG,'' 
+;c2sin0,  +)'2cos0j 

^  > 


(Eq.  6) 


Y 


This  functionality  is  extremely  usefully  in  dynamically  configuring  new  paths  from  our 
original  paths.  We  can  dynamically  define  another  path  depending  on  your  position  and  the 
direction  of  your  vehicle.  For  the  sonar  functions,  it  allows  much  more  modularity  to  the 
code.  The  code  is  reusable,  since  the  only  thing  unique  to  Yamabico  are  the  actual  sonar 
positions  on  the  robot. 
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C.  LINEAR  FEATURE  EXTRACTION 


In  addition  to  simple  range  and  point  position  data,  the  sonar  system  recognizes  the 
linear  features  of  an  orthogonal  world.  To  do  so  we  must  provide  some  method  for 
recognizing  sets  of  data  points  which  form  the  linear  feature  and  a  method  for  finding  and 
describing  the  line  segment  that  best  fits  that  set  of  data  points.  This  is  accomplished  in 
reverse  fashion,  i.e.  we  presume  the  data  we  are  receiving  belongs  to  such  a  set  and 
continuously  modify  a  descriptive  line  segment  to  a  best  fit  of  the  data  using  a  least  squares 
fitting  algorithm.  This  line  segment  continues  to  grow  until  the  incoming  data  or  certain 
measures  of  the  line  segment  indicate  that  the  line  segment  should  be  ended  and  a  new  one 
started. 

1.  Least  Squares  Fitting 

Suppose  we  have  collected  n  consecutive  valid  data  points  in  a  local  coordinate 
system,  (pj,...,  /?„),  where  pi  =  (xi,  y,)  for  i  =  We  obtain  the  moments  of  the  set 
of  points 


(Eq.  7) 

(Eq.  8) 

(Eq.  9) 

(Eq.  10) 

(Eq.  11) 
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We  adopt  the  parametric  representation  (r,a)  of  a  line  with  constants  r  and  a.  If  a 
point  p  =  (j:,  j)  satisfies  an  equation 

r  =  Jtcosa  +  ysina  (-Jt/2  <  a  ^  7t/2)  (Eq.  12) 

then  the  point  p  is  on  a  line  L  whose  normal  has  an  orientation  a  and  whose  distance  from 
the  origin  is  r  (Figure  6.).  This  method  has  an  advantage  in  expressing  lines  that  are 
perpendicular  to  the  X  axis.  The  point-slope  method,  where  y  =  mx  +  b,  \s  incapable  of 
representing  such  a  case  {m  =  oo,b  is  undefined). 


The  residual  of  point  p,-  =  (x,-,  y^)  and  the  line  L  =  (r,a)  is  A:,.cosa  +  3»,.sina-r. 
Therefore,  the  sum  of  the  squares  of  all  residuals  is 


5  =  ]^(r-x,.cosa-)-,.sina)^  ,  (Eq.  13) 

z=  I 

The  line  which  best  fits  the  set  of  points  is  supposed  to  minimize  S.  Thus  the 
optimum  line  (r,a)  must  satisfy 


dr  da 
Thus, 


(Eq.  14) 
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(Eq.  15) 


^  =  2V  (r-oc-cosa-j'.sina) 
dr  ^ 


f  ” 

f  "  ) 

(  ”  1 

=  2 

'•Si- 

cosa-  5]y,. 

sina 

^  i=l 

^/=1  ^ 

V,-=i  ^ 

/ 

=  2  ( rmgo  -  m  JO  cos  a -ffipj  sin  a) 
=  0 


and 


(Eq.  16) 
(Eq.  17) 


r  =  Z!!2cosa  +— sina  =  ^^cosa  +  n  sina  (Eq.  18) 

»ioo  '"oo  *  > 

where  r  may  be  negative.  Substituting  r  in  Equation  13  by  Equation  18, 

n 

S  =  ^  ( (JCj -  (>'1  “  ^ 

i  =  1 

Finally, 

n 

—  =  2y  ( (x.-nJ  cosa+  (y.—  fij  sina)  (-  sina+  (y,— iij  cosa)  (Eq.  20) 

da.  ^  ^ 

r  =  l 

=  2]^(  (yrM'y)^-  (Jc,-li;c)^)sin«cosa  +  22  (x,.-|x^)  (y.-R,,)  (cos^a-  sin^a)  (Eq.  21) 
/  =  1  *  =  1 

=  (Mo2  -  A/jo)  sin2a  +  2Af„  cos2a  (Eq.  22) 


=  0 

Therefore 


atan  (2Mj,/(Mo2-M2o))  ^Pq  23') 

a  -  2 

Equation  18  and  Equation  23  are  the  solutions  for  the  line  parameters  generated  by 
a  least  squares  fit. 


2.  Finding  Endpoints 

The  residual  of  a  point  pi  =  (Xj,  y,)  is 

5/ =  (P;, -j:,)  cosa  +  (la^-yj)  sina 
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(Eq.24) 


Therefore,  the  projection,  p'.  of  the  point  p,-  onto  the  major  axis  is 
P'i  =  (Jc,  +  8,.cosa,  +  6,.sina)  (Eq.  25) 

We  will  use  p\  and  p\  as  estimates  of  the  endpoints  of  the  line  segment  L  obtained 
from  the  set  p  of  data  points. 

3.  Residual  Testing 

We  wish  to  do  some  pre-filtering  of  the  data  in  order  to  remove  points  from  the  data 
stream  which  are  clearly  not  colinear  with  the  existing  points  of  set  p.  In  this  way  we  can 
often  detect  the  end  of  a  line  segment  before  having  to  perform  the  considerable 
computations  necessary  to  include  it  in  the  line.  If  the  point  satisfies 

6,.^ ,  <  max  (a  X  Cl,  C2)  (Eq.  26) 

where  Cl  and  C2  are  positive  constants  (typically.  Cl  =  0.02  and  C2  =  5.0)  then  the  point 
can  be  included  in  the  current  line  segment.  C2  at  5.0  allows  for  more  residual  at  a  distance 
greater  than  250  centimeters,  up  to  8  centimeters  at  a  distance  of  4  meters. 

4.  Beginning  Line  Segments 

First,  the  sonar  returns  must  fall  within  their  physical  constraints.  For  Yamabico, 
acceptable  return  values  fall  between  9.3  centimeters  and  409.0  centimeters.  If  a  sonar 
return  is  not  within  this  range,  a  segment  will  be  generated  if  there  have  been  at  least  10 
previous  returns  that  met  all  requirements  of  the  least  square  fitting  to  qualify  as  a  segment. 

Secondly,  if  it  is  the  first  return,  you  simply  store  it  as  the  starting  point  and  proceed 
with  the  next  return. 

With  the  line  segment  established,  collection  and  testing  of  the  additional  data 
points  can  proceed.  If  the  data  point  passes  the  residual  testing,  the  moments  and  test  values 
for  the  line  are  calculated  including  the  new  point.  Should  that  test  pass,  the  line  segment 
parameters  (endpoints,  length,  etc.)  are  updated  and  the  system  proceeds  to  gather  a  new 
data  point. 
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5.  Ending  Line  Segments 

There  are  three  ways  in  which  a  line  segment  is  ended.  It  may  be  ended  by  the 
failure  of  data  points  to  pass  the  residual  testing,  explicitly  ended  by  the  sonar  being 
disabled,  or  by  the  sonar  return  being  outside  the  acceptable  range. 
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IV.  MML  USER  INTERFACE 


A.  GLOBAL  CONFIGURATION  CALCULATION 

The  compose  function  is  implemented  in  a  sonar  function  called  calculate_global(). 
It  applies  the  compose  function  twice  [BYR94].  The  first  time  the  compose  function  is  used 
to  determine  the  actual  position  of  the  sonar  in  the  world  being  navigated  by  the  vehicle,  as 
seen  in  Figure  7.  In  this  example  Yamabico  is  at  coordinates  (80,40),  in  the  “world 
coordinates”.  The  sonars  position  on  the  robot  is  (9.5,  -19.75).  By  applying  the  compose 


Figure  7.  Example  of  First  Compose 


function. 


q\0(k^ 


Xj  +JC2COS0J 

+j:2sin0j  +y2C0sQ^ 
®l+®2 


we  determine  the  position  of  the  sonar  in  “world  coordinates”.  In  this  case  it  would  be: 
world  sonar  x  coordinate  =  100.68  =  80  +  9.5*cos(7t/4)  -  (-19.75*sin(7i/4)) 
world  sonar  y  coordinate  =  32.74  =  40  +  9.5*sin(7c/4)  +  (-19.75*cos(7c/4)) 
world  sonar  theta  =  -Ji/4  =  7t/4  +  -(7t/2). 

The  second  time  compose  is  applied  it  determines  where  the  sonar  return  is  in  the 
world  being  navigated  by  the  robot,  as  in  Figure  8. 

In  this  case  we  apply  the  compose  function  and  the  results  are: 
sonar  x  coordinate  from  robot  =  171.42  =  100.68  +  35*cos(-7C/4)  -  0*sin(-7c/4) 
sonar  y  coordinate  from  robot  =  -37.94  =  32.74  +  35*sin(-7i/4)  +  0*cos(-tc/4) 
which  gives  us  the  point  in  Figure  9. 

By  knowing  where  each  sonar  is  on  the  vehicle  and  knowing  where  the  vehicles 
position  is,  we  can  consistently  determine  where  the  object  being  detected  is  in  relation  to 
the  world  that  Yamabico  is  in.  This  is  needed  so  that  a  vehicle  can  dynamically  map  out  the 
world. 
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B.  SONAR  FUNCTIONS 


Sonar  functions  are  found  in  sonarcardx,  sonarmath.c,  sonario.c,  sonarsys.c,  and 
sonarlogx,  which  are  part  of  Yamabico’s  MML,  the  name  for  the  entire  set  of  code  for 
Yamabcio.  The  following  are  those  functions  which  are  available  for  use  in  the  userx  and 
a  brief  description. 

1.  Enable  Sonar 

Syntax:  void  enable_linear_fitting(n) 
intn; 

Description: 

The  user  calls  this  function  passing  in  the  sonar  that  is  to  be  enabled.  On  Yamabico 
there  are  12  available  sonars.  Each  sonar  should  be  enabled  individually. 

2.  Disable  Sonar 

Syntax:  void  disable_sonar(n) 
int  n; 

Description: 

The  user  calls  this  function  passing  in  the  sonar  that  is  to  be  disabled.  On  Yamabico 
there  are  12  available  sonars.  Each  sonar  should  be  enabled  individually. 

3.  Get  Sonar  Returns 

Syntax:  double  sonar(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number  that  range  data  is  wanted 
from.  If  no  echo  is  received,  then  an  INFINITY(1.0e6)  is  returned.  If  the  distance  is  less 
than  10  cm,  then  a  0  is  returned.  If  the  sonar  return  is  between  9  cm  to  409  cm,  then  that 
floating  point  number  will  be  returned  in  centimeters. 
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4.  Get  Global  Sonar  Returns 


Syntax:  posit  global(n) 
int  n; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number  that  global  range  data  is 
wanted  from.  The  function  will  return  a  structure  of  type  posit,  which  contains  gx  and  gy, 
the  global  x  and  y  coordinates. 

5.  Enable  Linear  Fitting 

Syntax:  void  enable_linear_fitting(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number,  so  that  linear  fitting  is 
applied  to  sonar  returns.  This  will  enable  the  robot  to  determine  whether  sonar  returns  are 
walls,  or  some  type  of  linear  surface. 

6.  Disable  Linear  Fitting 

Syntax:  void  disable_linear_fitting(n) 

int  n; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  that  linear  fitting  is  to  be  disabled 
on. 

7.  Set  Parameters  In  Linear  Square  Fitting 

Syntax:  void  set_sonar_parameters(cl,  c2) 

float  cl, c2; 

Description: 

Allows  the  user  to  adjust  constants  which  control  the  linear  fitting  algorithm.  Cl  is 
a  multiplier  to  allow  more  leniency  for  greater  sonar  ranges,  and  C2  will  adjust  the 
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tolerance  allowed  for  sonar  ranges  being  off  the  linear  line  being  collected.  Both  are  used 
to  determine  if  an  individual  data  point  is  usable  for  the  algorithm.  The  default  values  are 
initialized  to  0.02  and  5.0  respectively.  For  more  information  on  Cl  and  C2  refer  to  Chapter 
V.C  of  this  thesis. 

8.  Enable  Data  Logging 

Syntax: 

void  enable_data_logging(n,filetype,filenumber) 
int  n,filetype,filenumber; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar,  the  type  of  file  data  to  be 
collected,  and  which  file  array  (0,  1,  2,  or  3)  to  collect  the  data  in.  There  are  three  types  of 
file  data  that  can  be  collected.  The  first  is  raw  data,  the  second  is  global  data,  and  the  third 
is  segment  data. 

9.  Disable  Data  Logging 

Syntax:  void  disable_data_logging(n,filetype) 
int  n,  filetype 
Description: 

The  user  calls  this  function  and  passes  in  the  sonar,  the  type  of  file  data  to  collected, 
and  which  file  array  (0,  1,  2,  or  3).The  type  of  file  data  that  is  to  cease  being  collected  is 
designated,  either  raw  data,  global  data,  or  segment  data. 

10.  Set  Logging  Interval 

Syntax:  void  set_log_interval(n,d) 
int  n,  d; 

Description: 

The  user  calls  this  function  passing  an  integer  designating  how  often  the  sonar  data 
being  collected  should  be  written  to  the  file  collecting  the  data.  The  default  value  is  13, 
which  for  a  speed  of  30  centimeters  per  second  and  sonar  sampling  time  of  25  milliseconds. 
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would  record  a  data  point  approximately  every  10  cm.  To  collect  all  sonar  data  you  pass  in 
1,  so  that  every  sonar  return  is  recorded. 

11.  Transfer  Raw  Data  To  Host 

Syntax:  void  xfer_raw_to_host(filenumber, filename) 

int  filenumber,  filename; 

Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1, 2,  or  3)  and  the  name 
of  the  file  that  is  to  be  created  at  the  workstation  to  contain  the  raw  sonar  data  collected. 

12.  Transfer  Global  Data  To  Host 

Syntax:  void  xfer_global_to_host(filenumber, filename) 
int  filenumber,  filename; 

Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1, 2,  or  3)  and  the  name 
of  the  file  that  is  to  be  created  at  the  workstation  to  contain  the  global  sonar  data  collected. 

13.  Transfer  Segment  Data  To  Host 

Syntax:  void  xfer_segment_to_host(filenumber,  filename) 

int  filenumber,  filename; 

Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1, 2,  or  3)  and  the  name 
of  the  file  that  is  to  be  created  at  the  workstation  to  contain  the  segment  sonar  data  collected 

C.  DATA  LOGGING  PROCEDURE 

After  Yamabico  has  completed  its  mission,  recorded  sonar  data  can  be  downloaded 
and  checked  to  ensure  that  the  hardware  is  performing  optimally.  The  data  that  can  be 
logged  includes  global  sonar  data,  raw  sonar  data,  segment  sonar  data,  and  the  motion  trace 
data  of  the  robot.  Once  the  robot  has  stopped,  the  data  designated  to  be  logged  in  user.c  can 
now  be  downloaded.  A  message  on  the  powerbook  will  instruct  the  user  to  connect  the 
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phone  cable  to  the  robot.  Once  the  phone  line  is  connected,  the  user  must  hit  the  space  bar, 
then  the  character  g,  and  the  space  bar  once  more.  The  data  will  then  be  downloaded  to  the 
workstation.  Once  the  download  is  completed,  a  bell  sound  will  be  heard  from  the 
powerbook  on  the  robot.  This  is  required  for  each  type  of  data  being  logged. 
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V.  SONAR  CHARACTERISTICS  EXPERIMENT  RESULTS 


To  successfully  use  sonars  in  motion  planning  and  obstacle  avoidance,  it  is 
necessaiy  to  understand  what  sonar  data  you  can  expect  in  distinct  rectilinear 
configurations.  This  allows  determination  of  which  cases  will  successfully  avoid  obstacles, 
and  which  cases  will  be  unable  to  determine  a  safe  path  given  only  input  from  the  sonars. 

Several  motion  planning  experiments  were  conducted  in  Spanagel  Hall  at  the  Naval 
Postgraduate  School.  During  all  cases  the  left  and  right  sonars  (#5,  #7)  were  enabled.This 
provided  complete  scans  of  the  world  in  which  the  robot  was  moving.  The  Linear  Fitting 
Algorithm  constructs  segments  from  individual  sonar  returns. 

The  Linear  Fitting  Algorithm  has  5  cm  allowance.  This  means,  a  segment  is 
continually  generated  until  the  new  return’s  distance  (image)  to  the  already  formed 
segment  exceeds  the  5  cm  allowance  of  the  algorithm  (Figure  10.). 


Segment  — 
Sonar  return 


Wall 


Figure  10.  Allowance  in  Linear  Fitting  Algorithm 
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A.  CASE  1 


The  robot  moves  using  its  left  and  right  sonars  in  a  translational  scan  as  in  Figure 
1 1 .  We  expect  to  receive  accurate  data  to  recognize  the  wall.  The  Linear  Fitting  Algorithm 
recognizes  two  walls.  More  importantly,  the  locations  of  the  doorway  cavities  are  correctly 
determined. 
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Thus,  in  case  1  the  robot’s  understanding  of  the  world  is  precise  and  the  result 
exhibits  good  linearity.  Figure  12.  shows  the  experiment  results. 
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B.  CASE  2 

Case  2  is  designed  to  observe  the  behavior  of  the  “EnableLinearFitting”  function, 
as  depicted  in  Figure  13.  The  robot  is  in  a  translational  scan  of  a  wall  which  ends  in  a 
comer.  We  expect  results  that  will  not  accurately  depict  the  comer. 


31 


y-axis 


Figure  13.  Case  2 

The  robot  moves  parallel  to  the  wall.  Prior  to  reaching  the  comer,  many  sonar 
returns  contributed  to  the  segment.  A  sonar  return  immediately  after  the  robot  passed  the 
comer  was  added  to  the  segment.  This  is  within  the  5  cm  allowance.  However,  the  accuracy 
of  the  generated  line  segment  is  not  significantly  affected.  The  experiment  results  is  shown 
in  Figure  14. 
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Figure  14.  Case  2-Experiment  Results 


C.  CASE  3 

The  robot  uses  its  right  sonar  in  a  translational  scan  of  a  comer  in  a  situation  shown 
in  Figure  15.  The  expected  results  may  not  accurately  depict  the  comer  due  to  the  low 
amount  of  reflection  which  distorts  the  representation  of  the  angle  of  the  wall. 


33 


Figure  15.  Case  3 


The  actual  results  showed  the  expected  angular  distortion.  Some  sonar  returns  from 
the  wall  are  received  before  the  robot’ s  y-coordinate  reaches  the  y-coordinate  of  the  comer. 
Next,  as  the  robot  move  parallel  to  the  wall  we  get  more  sonar  returns.  These  returns  along 

with  the  prior  ones,  generate  a  first  segment. 

Then,  another  segment  is  generated  right  after  the  Linear  Fitting  Algorithm’s 
allowance  prohibits  the  first  segment  to  continue.  The  two  segments  together  give  us  a 
bended  representation  of  the  wall.  The  experiment  results  is  shown  in  Figure  16. 
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VI.  THEORY  OF  POLYGONS 


This  chapter  presents  definitions  used  in  the  development'  of  the  theory  and 
includes  the  concepts  of  polygons,  subpolygons,  and  rectilinear  worlds.  Basic  terminology 
and  definitions,  which  form  the  basis  of  the  discussions  to  follow,  are  now  covered 
[KAN95a]  [KAN95c]. 

A.  DISTANCE  AND  BISECTORS 

We  assume  a  global  two-dimensional  Cartesian  coordinate  system  in  a  plane. 
Distances  will  be  measured  as  Euclidean  distance.  The  distance  dip,  q)  between  two 
points  p=  and  q=  is  defined  by  the  usual  norm: 

dip, q)  =  Jix^-xy+iyp-y/  (Eq.  27) 

Assume  there  are  n(n>2)  points  in  a  plane  that  make  up  the  world,  W. 

{Pv->Pn}  •  (Eq.28) 

The  bisector  of  two  points  is  a  straight  line  which  bisects  and  is  perpendicular  to  a 
line  connecting  the  two  points.  A  bisector  bsip^pp  of  points  p.  and  p.  with 

l<i,j<n,i^j  is  defined  as 


bsiPi,  Pj)  =  {pi  dip,  pP  =  dip,  pp}  (Eq.  29) 

Bisectors  play  important  roles  in  this  theory  in  several  ways.  Obviously,  bsip^,  pp  is  a  line 
for  every  pair  (p,.,  pp  of  points. 

Voronoi  regions  are  those  regions  in  which  any  point  in  the  region  is  closer  to  an 
obstacle  than  any  other  obstacle.  As  is  well-known,  the  Voronoi  region  Vipp  of  a  point  p,. 

with  1  <  i  <  n  is  defined  as 
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^(P,)  =  {pi  i'^j)d(p,p,)^d(p,pp} 


(Eq.  30) 


B.  POLYGONAL  WORLDS 

Throughout  this  discussion  we  will  use  the  rectilinear  world  illustrated  in  Figure  17. 


Figure  17.  Example  World 


as  a  concrete  example  of  the  concepts  developed  herein.  Note  that  the  inner  white  regions 
indicate  areas  in  free  space  while  the  darker  areas  indicate  the  rectilinear  obstacles. 

1.  Polygons 

Let  a  polygon,  r ,  be  defined  by  an  ordered  circular  list  of  vertices,  located  in  a  two 

dimensional  plane,  91^ ,  satisfying  the  following  conditions: 

i)  any  three  consecutive  points  in  the  sequence  are  not  colinear, 

ii)  two  distinct  edges  and  (v,.,v,.^.,)  do  not  have  intersections  except 

in  the  case  that  they  share  a  common  end  point. 

■  Therefore,  polygon,  r ,  is  defined  as: 

r=  (vj . v,),Z>3  (Eq.  31) 
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where  r  consists  of  the  set  of  points  which  are  either  on  the  boundary  of  r  or  in  the  interior 
of  r .  The  boundary  of  the  polygon  r ,  denoted  3r ,  consists  of  those  points  of  a  straight 
edge  connecting  the  vertices  in  order.  The  last  vertex,  v, ,  is  connected  to  the  first  vertex  to 
close  the  directed  loop.  In  this  theory,  free  space  will  always  exist  to  the  right  of  the  directed 
boundary  loop.  The  interior  of  r ,  denoted  lnt{T)  ,  is  defined  as  the  set  of  points  to  the 
left  of  the  boundary.  The  exterior  of  r ,  denoted  Free  (F)  ,  is  defined  as  the  set  of  points 

to  the  right  of  the  boundary  and  can  therefore  be  defined  as  -  r  .  A  normal  polygon  is 
a  polygon  whose  ordered  list  of  vertices  produces  a  counterclockwise  boundary  loop. 
SeeFigure  18.  Normal  polygons  will  therefore  represent  obstacles  inside  the  boundaries  of 


Exterior 


Free  Space 

Figure  18.  Normal  Polygon 

the  world.  An  inverted  polygon  is  a  polygon  whose  ordered  list  of  vertices  produces  a 
clockwise  directed  boundary  loop.  See  Figure  19.  An  inverted  polygon  serves  as  the  outer 
boundary  of  the  world.  Recall  that  an  assumption  used  throughout  is  that  the  world  is 
bounded  by  an.  outer  inverted  polygon.  A  rectilinear  polygon  is  a  polygon  in  which  every 
edge  is  parallel  to  either  the  global  x  or  y  orthogonal  axes. 

Definitions  of  concave  and  convex  with  regard  to  vertices  and  polygons  are  covered 
here.  The  exterior  angle  will  serve  as  a  measure  to  determine  whether  a  vertex  is  concave 
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Figure  19.  Inverted  Polygon 

or  convex.  (See20. .)  Let  v|r  (v,.,  v,.^ ,)  represent  the  direction  in  global  coordinates  from  v,. 


to  V;^,  .  The  exterior  angle,  5,- ,  induced  at  the  ith  vertex,  ,  is  defined  as: 

8,  =  normalize  {Vf  (V;,  v,-^  ])->]/  (v,_  p  v.) )  (Eq.  32) 

Note  that  the  difference  between  the  directions  is  normalized  to  fall  within  {-n,K]  .A 
vertex,  v,. ,  is  said  to  be  a  convex  vertex  if  8,.  >  0  .  Otherwise,  the  vertex  is  said  to  be  a 
concave  vertex.  A  polygon  is  said  to  be  a  convex  polygon  if  all  of  its  vertices  are  convex. 
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otherwise  it  is  a  concave  polygon.  For  more  details  on  the  properties  of  polygons  see  [30]. 
Several  examples  are  presented  in  the  Figures  below.  Figure  2 1 .  shows  examples  of  convex 
polygons,  while  Figure  22.  shows  examples  of  concave  polygons. 


2.  Subpolygons 

Let,  r  =  (vj, ..., V;),/>3  be  a  polygon.  It  is  desired  to  decompose  r  into 

smaller  pieces,  called  subpolygons.  At  this  point  the  decomposition  is  dealing  with 
decomposing  polygons  into  subpolygons  and  is  not  a  decomposition  of  free  space.  It  is, 
however,  a  step  in  the  future  process  of  free  space  decomposition. 

ff  the  polygon  r  is  convex,  i.e.,  if  all  of  the  vertices  are  convex,  we  stipulate  that 
the  polygon  r  itself  is  a  unique  subpolygon  in  r .  If  r  is  concave,  i.e.,  if  there  is  at  least 
one  concave  vertex  in  r ,  the  polygon  can  be  broken  up  into  one  or  more  subpolygons.  In 
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that  case,  the  first  vertex  in  the  subsequence  of  vertices  defining  a  subpolygon  is  set  as  one 
of  the  concave  vertices.  The  subsequence  continues  until  it  encounters  another  concave 
vertex,  which  becomes  the  last  vertex  in  the  subpolygon’s  defining  subsequence.  A 
subsequence 

Y  =  (v,,v,^,,  ...,v„),?<M  (Eq.  33) 

of  r  is  said  to  be  a  subpolygon  of  r ,  if  v,  and  v„  are  concave  and  if  all  the  vertices 
v,+ 1,  •••,  v„_ ,  are  convex,  v,  and  are  said  to  be  the  end-vertices  of  the  subpolygon  r . 
In  a  special  case  where  there  is  only  one  concave  vertex  Vj  in  r , 

(v,,  ...,v,,  V])  (Eq.  34) 

is  the  unique  subpolygon. 

Figure  23.  shows  the  decomposition  of  the  polygons  in  the  example  world 


Figure  23.  A  Rectilinear  Polygonal  World 


into  subpolygons.  Note  that  Fj ,  which  is  a  concave  polygon,  consists  of  six  subpolygons. 


42 


The  encompassing  shape  of  each  subpolygon  that  makes  up  r,  is  shown  using  a  dotted  line 

with  arrowhead  ends.  The  other  convex  polygons  consist  of  only  one  subpolygon  each. 
Additional  examples  of  polygonal  decomposition  into  subpolygons  are  given  in  Figure  24. 


Figure  24.  Subpolygon  Decomposition  of  Concave  Polygons 


The  polygon  on  the  left  of  the  figure  consists  of  two  subpolygons.  The  polygon  on  the  right 
of  the  figure  consists  of  only  one  subpolygon. 

Lemma  1:  Any  polygon  r  is  uniquely  divided  into  a  finite  number  of  subpolygons 
(Yi>  Yz’  •••’ Yfi)  (Eq.  35) 

with  keeping  the  order  of  occurrences  of  vertices  in  r .  Each  convex  vertex  v  in  r  belongs 
to  one  and  only  one  subpolygon. 

If  two  subpolygons  y  and  y  share  the  same  end- vertex,  they  are  said  to  be  adjacent 
and  we  write  adj(y,y)  .  For  example,  in  Figure  23.,  y,  is  adjacent  to  y^ .  Also,  y^  is 

adjacent  to  y^ .  However,  y,  is  not  adjacent  to  y^ . 

3.  Distance  from  a  Point  to  a  Subpolygon 

In  a  polygonal  world  tV,  let  p  be  a  point  in  its  free  space  and  y  one  of  the 
subpolygons  in  W  respectively.  Then  d(p,  y)  means  the  minimal  visible  distance  from  p 
to  y .  If  p  and  y  are  not  visible  to  each  other  in  this  world,  d(p,  y)  =  <=0  . 
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The  image,  im  {p,  y.)  ,  of  the  point  p  on  the  subpolygon  y,-  is  defined  as  the  closest 
point  on  y,  from  p.  The  image  may  lie  on  a  convex  vertex  or  within  an  edge  of  a 
subpolygon.  The  im(p,y,.)  will  have  as  components  the  global  x  and  y  coordinates, 
denoted  em(p,y,)  j 

Proposition  1:  For  any  point  p  and  a  subpolygon  y  in  a  world,  if  y  has  no  vertex 
in  the  interior  of  the  convex  hull  of  T  which  contains  y,  then  the  image  of  p  on  y 
is  unique. 

Proof;  Since  there  are  no  vertices  of  y  in  the  interior  of  the  convex  hull  of  r ,  then 
by  definition  of  a  convex  hull,  the  vertices  of  y  must  lie  on  the  boundary  of  the 
convex  hull.  Since  the  convex  hull  is  a  convex  polygon,  then  there  is  only  one  image 
point  to  y . 

4.  Polygonal  World 

Assume  a  world  W  is  given  which  consists  of  a  finite  number  of  polygons  and  each 
polygon  is  divided  into  one  or  more  subpolygons.  Assume  there  are  n  non-overlapping 
polygons  in  the  world. 


w=  {r„...,r„},n>i 

(Eq.  36) 

w  =  ur,- 

i 

(Eq.37) 

int(W)  = 

i 

(Eq.38) 

Free{W)  =  KJEreeiTi)  = 

(Eq.  39) 
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Free{W)  is  called  free  space  of  W.  dw  is  defined  as  the  boundary  of  W.  lnt(,W)  is 
defined  as  the  interior  of  W.  A  rectilinear  world  is  a  world  in  which  every  polygon 
contained  in  the  world  is  rectilinear. 
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Vn.  GLOBAL  MOTION  PLANNING /CONVEX 


DECOMPOSITION 

A.  PATH  CLASS 

A  world  W  =  {Bq,  n>=l,  consists  of  n  simple  polygons  (holes)  and 

another  simple  polygon  Bq  which  defines  the  outmost  boundary  [KAN95b].  The^ee  space 
of  W  is  the  inside  of  Bq  minus  the  union  of  the  other  n  polygons’  inside  and  is  denoted  by 

Free(w).  (Although  polygons  and  borders  can  be  non  rectilinear  in  this  theory,  all  examples 
given  are  orthogonal  for  simplicity.) 

A  path  in  W  is  a  continuous  function  f:  [0,1]  ->  Free(W).  The  two  points  f(0)  and 
f(l)  are  called  its  endpoints.  If  they  are  distinct,  we  usually  denote  f(0)  as  a  start  S  and  f(l) 
goal  G.  We  assume  that  f  is  rectifiable  (its  length  is  finite).  Two  paths  /  and  /  with  the 
same  endpoints  are  said  to  be  homotopic  if  /  can  be  continuously  transformed  into  / 
without  moving  both  endpoints  and  without  running  over  any  polygons.  If  /  and  /  are 

homotopic,  we  write /s/  .  In  Figure  25.,  fi=f2  and  The  relation  =  is  an 
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equivalence  relation  and  defines  equivalence  classes  of  paths  that  share  the  same  endpoints. 
These  equivalence  classes  are  called  path  classes. 


Figure  25.  Paths 


The  motion  planning  problem  is  a  problem  of  finding  the  “optimal”  motion  for  a 
robot  given  a  world  W  and  a  pair  of  configurations  (position  and  direction).  We  divide  this 
problem  into  two:  (i)  finding  the  “optimal”  path  classes  (the  global  motion  planning 
problem)  and  (ii)  finding  the  “optimal”  motion  in  the  given  optimal  path  class  {local  motion 
planning  problem).  This  thesis  mainly  deals  with  the  first  problem.  It  is  known  that  the 
number  of  path  classes  goes  faster  than  any  polynomial  function  of  n,  where  n  is  the  number 
of  holes  in  the  world.  Therefore,  we  must  find  an  efficient  algorithm  for  robot  motion 
planning. 

B.  DECOMPOSITION 

The  boundary  3  W  of  a  world  is  defined  as  the  union  of  all  polygon  boundaries  in 
W.  A  border  in  a  world  is  a  straight  line  segment  L,  (i)  its  both  endpoints  are  on  the 
boundary  3  W  of  the  world,  and  (ii)  the  open  segment  L  is  a  subset  of  the  free  side  Free(w) 
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of  the  world.  A  finite  set  D  of  borders  in  W  is  called  a  decomposition  of  the  world,  if  D 
satisfies  the  condition  that  if  any  two  borders  share  a  point,  it  is  one  of  the  endpoints  for 
each  border.  Figure  26.  shows  two  ways  of  decomposing  the  world  shown  in  Figure  25. 


(Tree  Decomposition)  (Convex  Decomposition) 


Figure  26.  Decompositions 

When  we  say  a  path  f  intersects  a  border  L,  we  also  specify  the  orientation  how  f 
intersects  it.  By  adding  an  orientation  to  L,  we  say  f  positively-intersects  or  negatively- 

intersects  L  depending  on  the  direction  of  f  An  oriented  border  is  represented  by  L  or 

L  (Although  the  way  an  orientation  is  defined  upon  a  border  is  arbitrary,  we  follow  this 
convention:  We  give  an  orientation  to  each  border  so  that  if  a  path  intersects  a  horizontal 
border  upward  or  it  intersects  a  vertical  border  rightward,  we  say  the  path  positively- 

intersects  the  border  (L  ),  otherwise,  negatively-intersects  ( L  ").) 

Given  a  path  f  in  a  decomposed  world,  we  define  its  border  sequence  A,  (/) ,  as  an 

ordered  list  of  oriented  borders  which  f  intersects.  For  instance,  in  Figure  27.,  the  border 

sequence  of  the  two  paths  f  1  and  f2  are 

>.(/,)=  {L,+L6+  ) 

{^2  +  L^  +  L^+  ) 
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In  Figure  27.,  the  positive  orientation  of  each  border  is  shown  by  an  arrow.  If  either 
of  the  endpoints  is  on  a  border,  this  oriented  border  is  also  included  in  its  border  sequence. 

If  f  does  not  intersects  any  borders  and  if  both  endpoints  of  f  are  not  on  any  border,  X,  (/)  is 
defined  as  the  empty  sequence  e.  If  f  stays  on  one  border,  X,  (/)  =  e . 


Figure  27.  Paths  in  a  Decomposed  World 


The  border  sequence  of  a  path  f  in  Figure  28.  is 

X,  (/)  =  {Z/2  +  Z<2  ~  7*]  +  +  ) 

This  kind  of  a  path  can  be  simplified,  because  this  path  is  homotopic  to  the  path  f\ 
in  Figure  27.  Another  example  of  an  unreasonable  path  is  one  which  has  infinitely  many 
intersections  with  D.  Through  the  following  definition  and  lemma,  we  can  avoid  dealing 
with  these  pathological  paths.  A  path  f  in  a  decomposed  world  is  said  to  be  regular  if  it 
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intersects  borders  for  any  finite  number  of  times  and  its  border  sequence  "k  (J)  does  not  have 
any  subsequence  L  ^  L  ~  oxL  ~  L  *  . 


Figure  28.  A  Path  which  is  not  Regular 


Lemma  1  For  any  path  f  in  a  decomposed  world,  there  exists  a  regular  path  f 
such  that  /=/ . 

C.  HOMOTOPIC  DECOMPOSITION  AND  PATH  REGION 

Each  portion  of  the  free  space  Free(W)  divided  by  a  decomposition  D  is  called  a 
region.  A  decomposition  D  of  a  world  is  said  to  be  a  homotopic  decomposition  if  all  the 
polygons  in  W  are  connected  through  the  borders  in  D.  All  decompositions  in  Figure  26., 
Figure  27.,  and  Figure  28.  are  homotopic  decompositions. 

Lemma  2  Each  region  in  a  homotopically  decomposed  world  is  contractible  ( or 
does  not  contain  any  holes). 

The  Free(W)  may  not  be  divided  by  a  homotopic  decomposition  D  at  all.  The  first 
decomposition  Dj  in  Figure  26.  is  such  an  example.  If  a  homotopic  decomposition  does  not 
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divide  the  free  space  into  more  than  one  region,  this  decomposition  is  called  a  tree 
decomposition. 

Proposition  1  A  homotopic  decomposition  of  a  given  world  with  the  minimum 
number  of  borders  is  a  tree  decomposition.  The  number  of  borders  in  a  tree  decomposition 
and  the  number  of  holes  in  the  world  are  the  same.  For  an  arbitrary  world,  there  exists  at 
least  one  tree  decomposition. 


Figure  29.  Oriented  Region 


Suppose  Z1Z2  is  a  subsequence  of  the  border  sequence  (/)  of  a  path  f,  where  Zj 
and  Z2  are  oriented  borders.  Then  there  is  a  region  R  such  that  f  enters  into  R  through  the 
oriented  border  zj  and  exits  R  through  the  oriented  border  Z2.  We  call  the  borders  Zj  and 
Z2  the  entrance  and  the  exit  of  the  region  R  in  relation  to  the  path  f.  Now  we  define  an 
oriented  region  form  the  region  R  by  artificially  closing  all  borders  belonging  to  R  except 
for  the  entrance  and  exit.  The  borders  which  are  artificially  closed  are  called  virtual 
boundaries  of  the  oriented  region.  Thus,  an  oriented  region  is  bounded  by  (i)  entrance,  (ii) 
exit,  (iii)  portions  of  the  world  boundary,  and  (iv)  virtual  boundaries.  The  union  of  the  parts 
(iii)  +  (iv)  is  divided  into  two:  the  left  boundary  and  right  boundary  of  the  oriented  region. 
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A  left  or  right  boundary  of  an  oriented  region  may  be  one  point,  but  cannot  be  empty 
(Figure  29.). 

The  initial  region  of  a  path  f  is  the  region  where  S  belongs.  If  S  is  on  a  border,  the 
initial  region  is  empty.  The  final  region  of  a  path  is  the  region  where  G  belongs.  If  G  is  on 
a  border,  the  final  region  is  empty.  (The  left/right  boundaries  of  the  first  or  last  regions  are 
not  defined,  since  they  have  the  exit  or  the  entrance  only,  not  both.) 

The  path  region  of  a  path  is  the  union  of  the  first  region,  all  the  oriented  regions, 
and  the  last  region  of  the  path.  The  left  (right  resp.)  boundary  of  a  path  region  is  the  union 
of  the  left  (right  resp.)  boundaries  of  all  the  oriented  regions.  A  path  region  is  also 
contractible.  The  path  region  of  the  path  f2  in  Figure  27.  in  a  decomposed  world  by  D2  as 

shown  in  Figure  26.  becomes  the  region  shown  in  Figure  30.  (If  a  path  region  is  constructed 
on  a  tree  decomposed  world,  it  becomes  extremely  complex.  This  is  one  reason  why 
convex  decompositions  are  more  appropriate  in  practical  work). 

A  path  region  is  a  geometric  representation  of  path  classes.  Therefore,  this  concept 
of  path  region  is  essential  in  providing  the  main  result  which  follows.  This  concept  is  also 
extremely  useful  in  the  local  motion  planning  problem,  although  this  thesis  does  not 
address  this  topic. 


Figure  30.  Path  Region 
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In  the  following  proposition  we  use  the  border  sequence  as  a  symbolic 
representation  of  path  classes. 

Proposition  2  (Main  Result)  Suppose  a  homotopically  decomposed  world  W  and 
two  endpoints  in  Free(W)  are  given.  For  an  arbitrary  pair  of  regular  paths  f  and  f 
connecting  these  endpoints,  /=/  if  and  only  if  X(f)=  X(f) . 

A  sketch  of  the  proof.  (I)  Only-if  part:  Let  us  consider  the  path  region  of  f.  Since  / 
and  f  are  both  regular,  /  is  also  confined  in  that  path  region. 

(n)  If  part:  The  border  sequence  X  (f)  defines  a  path  region.  Since  the  path  region 
is  contractible  with  its  left  /  right  boundaries,  /=/  follows. 

D.  CONVEX  DECOMPOSITION 

The  connectivity  graph  method  described  in  the  previous  Section  works  for  any 
homotopic  decomposition.  However,  the  use  of  convex  decompositions  generally  gives 
better  results,  where  a  decomposition  D  is  said  to  be  a  convex  decomposition  if  each  region 
generated  by  D  is  convex.  The  reasons  are  (i)  convex  decompositions  makes  cost- 
evaluation  for  basic  connectivity  graph  easier,  and  (ii)  convex  decompositions  makes  the 
local  motion  planning  task  simpler. 

Proposition  3  Every  convex  decomposition  is  a  homotopic  decomposition. 
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Proposition  4  A  vertical  decomposition  or  horizontal  decomposition  is  a  convex 
decomposition. 


Figure  31.  Non-Orthogonal  Border 


In  the  world  shown  in  Figure  31.,  the  diagonal  border  is  better  than  any  vertical  or 
horizontal  ones  from  several  aspects.  This  observation  supports  the  advantage  of  convex 
decompositions  over  vertical  or  horizontal  decompositions. 
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VIII.  DIJKSTRA’S  ALGORITHM 


A.  CONNECTIVITY  GRAPH  OF  THE  WORLD 

The  global  motion  planning  problem  is  the  problem  of  finding  the  “optimal”  path 
class  to  connect  given  start  and  goal  configurations  [KAN95a].  The  homotopic 
decomposition  method  is  an  extremely  useful  tool  to  solve  this  problem.  The  connectivity 
graph  is  defined  for  a  homotopically  decomposed  world. 


Figure  32.  World  Decomposition 


A  node  is  assigned  for  each  border  in  the  convexly  decomposed  world.  When  two 
borders,  Lj  and  L2,  belong  to  the  same  region,  an  edge  is  created.  A  cost  for  this  edge  is 
defined  as  the  energy  (or  time)  for  the  vehicle  to  make  a  motion  from  Lj  to  L2  or  from  L2 
to  Lj.  Therefore,  these  edges  are  undirected.  This  cost  not  only  reflect  the  distance  but  the 
turn  needed  to  make  the  motion.  It  may  also  reflect  the  safety  in  the  region  (i.e.  if  the  region 
is  narrow,  the  cost  is  high).  Since  the  region  is  convex,  any  two  borders  in  a  region  are 


57 


visible  and  the  cost  evaluation  is  relatively  straightforward.  This  graph  is  called  a  basic 
connectivity  graph. 


Figure  33.  Basic  Connectivity  Graph 


Given  a  start  and  a  goal  configurations,  we  add  two  new  nodes,  S  and  G,  to  the  basic 
connectivity  graph  to  obtain  an  augmented  connectivity  graph.  A  directed  edge  from  S  to 
each  border  in  the  start  region  is  added  to  the  graph.  A  cost  of  the  edge  is  made  equal  to  the 
energy  needed  for  a  robot  to  make  a  motion  from  the  start  configuration  to  the  border. 
Likewise,  a  directed  edge  from  each  border  in  the  goal  region  to  G  is  added.  A  cost  of  the 
edge  is  made  equal  to  the  energy  needed  for  a  robot  to  make  a  motion  from  the  border  to 
the  goal  configuration.  Figure  34.  shows  an  augmented  connected  graph  for  the  convexly 
decomposed  world  in  Figure  32. 
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Figure  34.  Augmented  Connectivity  Graph 
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B.  ALGORITHM  DESCRIPTION 


Now,  finding  the  “optimal”  path  class  from  S  to  G  in  the  world  shown  in  Figure  32. 
is  transformed  into  the  minimum  cost  path  finding  problem  from  S  to  G  in  the  augmented 
connectivity  graph  shown  in  Figure  34.  The  Dijkstra’s  algorithm  can  be  perfectly  applied 
to  this  global  motion  planning  problem.  As  a  result,  a  border  sequence  is  obtained.  The 

computation  time  is  O  (A^)  using  two  for  loops  with  the  on  being  nested,  where  N  is  the 
number  of  nodes  (borders)  in  the  augmented  connected  graph  respectively. 

Minimum_Cost_Path_Finding(G,  s,  g) 
begin 

for  all  vertices  u  do 
u  .mark:=  0; 

u  .cost:=  CO 
s.cost:=  0; 

•u  :=  s; 
repeat 
-0  .mark:=  1 ; 

for  all  edges  (uO,  u)  €  £  do 
if  uO  .cost  +  c  (uO,  -u)  <  V  .cost  then 
u  .cost:=  t)0  .cost  +  c  (uO,  u)  ; 
t)  .prev:=  uO ; 

uO  :=  [  such  that  u  .cost  is  minimum  among  unmarked  u  ]; 
until  uO  :=  g; 

Figure  35.  Dijkstra’s  Algorithm 
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In  the  Dijkstras’s  algorithm  G  =  (V,  E)  is  the  augmented  connectivity  graph  with  a 
node  set  V  and  a  edge  set  E.  A  cost  c(u,  w)  >  0  is  given  to  each  edge  (u,  w).  The  problem 
to  be  solved  is  to  find  the  minimum  cost  path  from  s  to  g  and  the  cost  itself. 

For  each  node  u,  we  use  variables  u.mark,  u.cost,  and  u.prev  in  this  algorithm, 
u.cost  is  the  minimum  cost  from  s  to  u  known  so  far.  u.mark  is  set  to  1  when  u.cost  is  known 
to  be  the  minimum  cost,  u.prev  is  a  pointer  to  the  node  which  is  placed  to  the  previous 
position  in  the  shortest  path  from  s  to  u.  When  an  execution  of  this  program  terminates,  the 
minimum  cost  is  found  in  g.cost  and  the  shortest  path  is  obtained  in  a  reverse  manner 
starting  from  g.prev. 

C.  DATA  STRUCTURE 

To  completely  determine  a  graph,  we  need  to  know  the  number  of  nodes  in  it,  and 
the  neighbors  of  each  node.  This  observation  will  lead  us  to  specify  the  proper  data 
structure  for  our  implementation  of  Dijkstra’s  algorithm. 

The  connectivity  graph  is  represented  by  an  array  of  nodes,  GNodes[numOfNodes]. 
Each  node  in  the  graph  is  a  “structure”  and  has  as  a  member,  an  array  of  pointers  to  node, 
adj  Array  [numOfNodes].  This  way  we  can  keep  track  of  the  neighbors  of  each  node.  The 
details  of  the  data  structure  can  be  found  in  Appendix  A. 
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Figure  36.  Dijkstra’s  Data  Structure 


D.  IMPLEMENTATION 

We  applied  Dijkstra’s  algorithm  for  the  augmented  connectivity  graph  in  Figure  34. 
Figure  37.  shows  the  nodes  visited  during  the  search.  The  minimum  cost  is  found  following 
the  prev.  pointers  from  the  Goal  node.  There  is  only  one  path  that  leads  to  the  Start  node. 
Every  node  is  visited  (see  bold  circles).  Inside  each  node  the  u.cost  value  is  shown.  Every 
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node  has  a  not  NULL  previous  pointer  (u.prev).  The  nodes  and  the  pointers  for  the 
minimum  cost  path  are  highlighted. 


Figure  37.  Dijkstra’s  Example  Execution 
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IX.  ALL  -  PAIRS  MINIMUM  COST  PATHS  ALGORITHM 
A.  CONNECTIVITY  GRAPH  OF  THE  WORLD 

The  problem  to  be  solved  is  how  to  find  the  “optimal”  path  class?  In  order  to  solve 
this  problem,  we  need  to  have  a  method  to  evaluate  the  cost  of  each  path  class.  A  reasonable 
approximation  method  looks  translating  the  polygonal  world  into  a  graph  [KAN95b]. 

Given  a  world  W  with  a  homotopic  decomposition  D,  its  basic  connectivity  graph 
G=  (V,  E)  is  defined  as  follow:  V  is  a  set  of  nodes  or  vertices  and  E  a  set  of  edges. 

V=  {L  ■  |(L€jD)}, 

namely,  we  create  nodes  L'*’  and  L'  for  each  border  in  the  decomposition. 

E  =  {(zj,  Z2)  I  there  exists  an  oriented  region  in  D 

such  that  its  entrance  is  z^  and  its  exit  is  Z2}. 

These  edges  are  directed.  A  cost  for  this  edge  is  computed,  for  instance,  as  the 
energy  (or  time)  needed  to  move  the  vehicle  from  the  center  of  the  entrance  border  to  the 
center  of  exit  border  with  appropriate  vehicle  directions.  In  this  cost  evaluation,  the  cost  for 
both  translational  and  rotational  motions  should  be  included.  The  basic  connectivity  graph 
of  a  decomposition  shown  in  the  upper  half  of  Figure  38.,  is  translated  into  the  basic 
connectivity  graph  shown  in  the  lower  half  of  the  same  Figure  (a  cost  for  each  edge  is  not 
specifically  shown). 
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Figure  38.  A  Decomposition  and  Its  Basic  Connectivity  Graph 
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It  is  straightforward  to  compute  “all-pair  minimum  cost  paths”  on  this  graph  G.  By 
this  method  we  preprocess  the  minimum  cost  from  any  oriented  border  to  any  other 

oriented  border  in  O(N^)  ,  where  N  is  the  number  of  borders  in  D.  After  this 

preprocessing,  for  any  oriented  border  pair  (z,z') ,  the  cost  c(z,z')  is  obtained  in  a 

constant  time.  In  general,  c  {z,  z')  ^  c  (z',  z)  . 

So  far  this  preprocessing  is  done  independent  of  a  situation  with  a  given  start-goal 
configuration  (position  and  direction)  pair.  Given  two  end-configurations,  and  qg,  we 

create  an  augmented  connectivity  graph  based  on  the  basic  connectivity  graph  by  the 
following  procedure: 

1.  Add  nodes  and  qg  to  V.  An  orientation  +  or  -  is  not  needed. 

2.  Let  Rs  be  the  region  where  qs  belongs  to.  Let  Zgi,  Zs2, ...  be  the  possible  exit 
borders  belonging  to  R^.  Add  an  edge  (qg,  Zjj)  to  the  edge  set  E  for  each  i.  Evaluate  the  cost 
c(<ls’  Zsi)  using  some  simulation  technique  and  define  it  as  the  edge  cost.  This  task  is  called 
initial  portion  motion  planning. 

3.  Let  Rg  be  the  region  where  qg  belongs  to.  Let  Zg^,  Zg2,...  be  possible  entrance 
borders  belonging  to  Rg.  Add  an  edge  (Zgj,  qg)  to  the  edge  set  E  for  each  i.  Evaluate  the  cost 
c(Zgi,  qg)  using  some  simulation  technique  and  define  it  as  the  edge  cost.  This  task  is  called 
final  portion  motion  planning. 
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Figure  39.  Addition  for  Augmented  Connectivity  Graph 

If  the  start  and  goal  configurations  are  specified  as  shown  in  Figure  39.,  we  will  add 
two  nodes,  and  qg,  and  five  edges  -  as  shown  also  in  the  right  half  of  the  same  Figure  - 
to  the  basic  connectivity  graph.  The  result  is  an  augmented  connectivity  graph. 

Then  the  original  global  motion  planning  algorithm  is  transformed  into  a  minimum 
cost  path  finding  problem  from  qj  to  qg  in  the  augmented  graph.  If  the  numbers  of  edges 
added  in  Steps  2  and  3  are  M  and  K  respectively,  we  need  to  compare  MK  distinct  costs. 
In  a  given  world,  the  maximum  values  of  M  and  K  are  limited.  Therefore  the  cost  for  the 
graph  search  portion  is  constant.  The  most  time  consuming  task  is  the  initial/final  motion 
planning  described  in  Steps  2  and  3.  This  is  a  good  result  when  we  recollect  the  fact  that 
the  number  of  distinct  path  classes  is  beyond  any  polynomial  function  of  the  number  of 
obstacles  n. 

As  a  review,  first  we  divide  the  free  space  of  the  world  into  regions  using  borders. 
A  connectivity  graph  is  next  defined  to  represent  the  geometrical  relation  of  the  world  and 
to  represent  distinct  path  classes  symbolically.  In  this  graph,  the  borders  are  nodes  as 
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opposed  the  previous  method,  in  which  regions  are  used  as  nodes.  A  pair  of  borders 
(nodes),  which  belong  to  the  same  region,  is  connected  by  an  edge  in  the  connectivity 
graph.  A  cost  of  the  motion  between  the  two  borders  is  evaluated  by  simulation  and  given 
to  the  corresponding  edge.  We  use  the  notion  of  oriented  borders  for  more  descriptive 
representation  of  path  classes  in  the  connectivity  graph.  In  order  to  find  the  optimal  path 
class  the  All-Pairs  Minimum  Cost  Paths  algorithm  is  applied. 

B.  ALGORITHM  DESCRIPTION 

The  problem:  Given  a  directed  weighted  graph  G=(V,  E)  with  nonnegative  costs, 
find  the  minimum-cost  paths  between  all  pairs  of  vertices. 

The  solution:  There  is  an  induction  method  useful  to  solve  the  problem.  We  leave 
the  number  of  vertices  fixed,  and  we  put  restrictions  on  the  type  of  paths  allowed.  The 
inductions  addresses  the  removals  of  these  restrictions  on  the  paths  until,  at  the  end,  all 
possible  paths  are  considered.  We  label  the  vertices  from  1  to  IVI.  A  path  from  u  to  w  is 
called  a  k-path  if,  except  for  u  and  w,  the  highest-label  vertex  on  the  path  is  labeled  k.  In 
particular,  a  0-path  is  an  edge  (since  no  other  vertices  can  appear  on  the  path). 

Induction  hypothesis:  We  know  the  lengths  of  the  minimum  cost  paths  between  all 
pairs  of  vertices  such  that  only  k-paths,  for  some  k  <  m,  are  considered. 

The  base  of  the  induction  is  m=l,  in  which  case  only  direct  edges  can  be  considered 
and  the  solution  is  obvious.  We  assume  the  induction  hypothesis  for  m,  and  we  try  to  extend 
ittom+1. 


Algorithm  All-PairMinimumCostPaths 
Input: 

Cost  (an  n  *  n  adjacency  matrix  representing  a  weighted  graph). 

{Cost  [x,  y]  is  the  cost  of  the  edge  (x,  y)  if  it  exists,  and  Infinity  otherwise; 

Cost  [x,  x]  is  0,  for  all  x} 

Previous  (an  n  *  n  adjacency  matrix  containing  vertices). 

Initial  state  for  Previous: 

(Previous  [x,  y]  is  the  id  of  vertix  (y),  if  Cost[x,y]!=  Infinity. 

Previous  [x,  y]  is  Undefined  otherwise,  and  Infinity  is  assigned  to  that  entry.  } 

Output: 

At  the  end  matrix  Cost  contains  the  lengths  of  the  shortest  paths  and 
matrix  Previous  contains  the  neighbor  to  (x)  vertix,  to  be  tracked  in  the  route  for 
the  shortest  paths,  for  each  entry  [x,  y]. 

begin 

for  m  :=  1  to  n  do 
for  X  :=  1  to  n  do 
for  y  :=  1  to  n  do 

if  Cost[x,  m]  +  Cost[m,y]  <  Cost[x,  y]  then 

Cost[x,  y]  :=  Cost[x,  m]  +  Cost[m,y] 

Previous[x,  y]  :=  Previous[x,  m] 


end 


Figure  40.  All-Pairs  Minimum  Cost  Paths  algorithm 
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C.  DATA  STRUCTURE 


To  completely  determine  the  graph,  we  need  to  define  the  nodes  of  it  and  the 
neighbors  of  each  node.  We  are  using  a  two-dimensional  array,  named 
Cost[numOfNodes][numOfNodes].  This  array  provides  the  information  on  the  number  of 
nodes,  and  the  connectivity  between  them. 

Let  X,  y  nodes  in  the  connectivity  graph.  Cost  is  0  for  an  entry  (x,  y)  of  the  Cost 
matrix,  if  x  =  y  .  Cost  is  oo  for  an  entry  (x,  y),  if  x^y  and  the  two  nodes  are  not 
connected.  For  all  other  cases  we  assign  a  value  representing  the  energy  or  time  a  robot 
needs  to  get  from  node  x  to  node  y  of  the  graph. 

Then  we  are  using  a  two-dimensional  array,  named 
Previous[numOfNodes][numOfNodes].  This  array  provides  the  information  on  which  is 
the  actual  neighbor  of  each  node  (by  node-id),  and  will  keep  the  information  in  the  search 
for  the  minimum  cost  path  between  every  single  pair  of  nodes. 

For  an  entry  (x,  y)  of  the  Cost  matrix.  Previous  node  is  assigned  the  y-node’s  id,  if 
x:^y  and  the  two  nodes  are  connected.  Previous  is  assigned  o  in  all  other  cases. 

The  details  of  the  data  structure  can  be  found  in  Appendix  A. 

D.  IMPLEMENTATION 

We  applied  All-Pairs  Minimum  Cost  Paths  algorithm  for  the  connectivity  graph  in 
Figure  38.  The  input  file  contains  the  edge  cost  values  for  the  nodes.  When  two  nodes  are 
not  connected  the  value  of  99  is  used  for  oo . 

The  initial  settings  for  the  Cost  and  Previous  arrays  are  shown  in  Figure  41.  and 

Figure  42.  respectively.  (  and  matrices). 

The  result  Cost  and  Previous  arrays  are  shown  in  Figure  43.  and  Figure  44. 

respectively.  (  and  matrices).  The  details  of  the  implementation  program  can  be 

found  in  Appendix  A. 
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Figure  41 .  Cost  Array  -  Initial  Matrix  D 


Figure  42.  Previous  Array  -  Initial  Matrix 
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***  1  2  3  4  5  6  7  8  9  10  11  12  13 
******* 

1*0  34  2  32  4  30  6  28  35  27  3  31  7 
2'  *  58  0  60  26  26  88  28  86  1  57  61  25  29 

3  *  26  32  0  30  2  28  4  26  33  25  1  29  5 

4  *  60  2  62  0  28  90  30  88  3  59  63  27  31 

5  *  32  30  34  28  0  62  2  60  31  31  35  27  3 

6  *  26  4  28  2  30  0  32  26  5  25  1  29  33 

7  *  30  28  32  26  26  60  0  58  29  29  33  25  1 

8  *  28  6  30  4  32  2  34  0  7  27  3  31  35 

9  *  57  27  59  25  25  87  27  85  0  56  60  24  28 

10*  1  35  3  33  5  31  7  29  36  0  4  32  8 

11*  25  31  27  29  29  27  31  25  32  24  0  56  32 

12*  33  3  35  1  1  63  3  61  4  32  36  0  4 

13*  29  27  31  25  25  59  27  57  28  28  32  24  0 

14*  29  7  31  5  33  3  35  1  8  28  4  32  36 

15*  47  17  49  15  15  77  17  75  18  46  50  14  18 

16*  11  45  13  43  15  41  17  39  46  10  14  42  18 

17*  15  21  17  19  19  17  21  15  22  14  18  46  22 

18*  43  13  45  11  11  73  13  71  14  42  46  10  14 

19*  19  17  21  15  15  49  17  47  18  18  22  14  18 

20*  39  17  41  15  43  13  45  11  18  38  14  42  46 

21*  46  16  48  14  14  76  16  74  17  45  49  13  17 

22*  12  46  14  44  16  42  18  40  47  11  15  43  19 

23*  44  14  46  12  12  74  14  72  15  43  47  11  15 

24*  14  48  16  46  18  44  20  42  49  13  17  45  21 

25*  42  20  44  18  46  16  48  14  21  41  17  45  49 

26*  16  14  18  12  12  46  14  44  15  15  19  11  15 

27*  40  18  42  16  44  14  46  12  19  39  15  43  47 

28*  18  16  20  14  14  48  16  46  17  17  21  13  17 


14  15  16  17  18  19  20  21  22  23  .24  25  26  27  28 

•^  *****************  It*  ***** 

21  45  17  13  21  17  17  46  16  48  14  14  20  16  18 

85  11  47  71  15  39  75  12  46  14  44  72  42  74  40 

25  43  15  11  19  15  15  44  14  46  12  12  18  14  16 

87  13  49  73  17  41  77  14  48  16  46  74  44  76  42 

59  41  21  45  17  13  49  42  20  44  18  46  16  48  14 

25  15  15  11  19  43  15  16  14  18  12  12  46  14  44 

57  39  19  43  15  11  47  40  18  42  16  44  14  46  12 

27  17  17  13  21  45  17  18  16  20  14  14  48  16  46 

84  10  46  70  14  38  74  11  45  13  43  71  41  73  39 

28  46  18  14  22  18  18  47  17  49  15  15  21  17  19 

24  42  14  10  46  42  14  43  13  45  11  11  45  13  43 

60  14  22  46  18  14  50  15  21  17  19  47  17  49  15 

56  38  18  42  14  10  46  39  17  41  15  43  13  45  11 

0  18  18  14  22  46  18  19  17  21  15  15  49  17  47 

74  0  36  60  4  28  64  1  35  3  33  61  31  63  29 

38  56  0  24  32  28  28  57  27  59  25  25  31  27  29 

14  32  4  0  36  32  4  33  3  35  1  1  35  3  33 

70  24  32  56  0  24  60  25  31  27  29  57  27  59  25 

46  28  8  32  4  0  36  29  7  31  5  33  3  35  1 

10  28  28  24  32  56  0  29  27  31  25  25  59  27  57 

73  27  35  59  3  27  63  0  34  2  32  60  30  62  28 

39  57  1  25  33  29  29  58  0  60  26  26  32  28  30 

71  25  33  57  1  25  61  26  32  0  30  58  28  60  26 

41  59  3  27  35  31  31  60  2  62  0  28  34  30  32 

13  31  31  27  35  59  3  32  30  34  28  0  62  2  60 

43  25  5  29  1  25  33  26  4  28  2  30  0  32  26 

11  29  29  25  33  57  1  30  28  32  26  26  60  0  58 

45  27  7  31  3  27  35  28  6  30  4  32  2  34  0 


Figure  43 .  Cost  Array  -  Result  Matrix  D 
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*********************************^,^,^,i,^,^^^, 

22  11  26  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  26  17  28  25  19 
22  11  26  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  26  17  28  25  19 
22  11  26  13  27  15  24  21  26  17  28  25  19 
22  11  23  13  27  15  24  21  17  17  28  25  19 
22  11  26  13  27  15  24  21  26  17  28  25  19 
22  11  23  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  26  17  28  25  19 
22  11  26  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  26  17  28  25  19 
22  11  26  13  27  15  24  21  26  17  28  25  19 
22  11  23  13  27  15  24  21  17  17  28  25  19 
22  11  23  13  27  15  24  21  26  17  28  25  19 
0  11  26  13  27  15  24  21  17  17  28  25  19 
22  0  23  13  27  15  24  21  17  17  28  25  19 
22  11  0  13  27  15  24  21  26  17  28  25  19 
22  11  26  0  27  15  24  21  26  17  28  25  19 
22  11  23  13  0  15  24  21  17  17  28  25  19 
22  11  23  13  27  0  24  21  26  17  28  25  19 
22  11  26  13  27  15  0  21  17  17  28  25  19 
22  11  23  13  27  15  24  0  26  17  28  25  19 
22  11  26  13  27  15  24  21  0  17  28  25  19 
22  11  23  13  27  15  24  21  17  0  28  25  19 
22  11  26  13  27  15  24  21  26  17  0  25  19 
22  11  23  13  27  15  24  21  17  17  28  0  19 
22  11  26  13  27  15  24  21  26  17  28  25  0 


Figure  44.  Previous  Array  -  Result  Matrix 


X.  CONCLUSION 


A.  RESULTS 

1.  Sonar  Characteristics  Measurement 

Yamabico’s  basic  sonar  characteristics,  taken  by  translational  scanning,  showed 
reasonable  results  for  the  sonars.  The  experiments  taken  by  applying  the  left  and  right 
sonars.  The  results  indicate  the  sonars  have  a  high  degree  of  accuracy  while  the  robot  is 
moving.  Sonar  data  had  a  precision  of  1  to  2.5  cm  during  the  testing,  in  robot’s  distances 
from  the  object  100  cm  to  150  cm.  This  is  the  difference  between  some  dimension  of  an 
object  in  the  real  world  and  its  image  in  the  robot’s  understanding  of  this  world. 

The  coordination  is  successful  between  the  sonar  and  motion  systems.  Specifically, 
the  linear  fitting  algorithm  accurately  applies  sonar  data  to  build  line  segments  that 
represent  the  wall. 

2.  Global  Motion  Planning 

The  connectivity  graph  representing  a  model  world  was  given  as  input  to  two 
existing  algorithms;  Dijkstra’s  and  All-Pairs  Minimum  Cost  Paths.  For  the  physical 
environment  used  in  Yamabico  robot  motion  experiments,  this  connectivity  graph  is 
typically  a  sparse  one. 

To  find  a  minimum  cost  path  from  a  start  to  a  goal  configuration,  Dijkstra’s 
algorithm  applied  in  this  environment.  Many  unnecessary  comparisons  between  each 

node’s  non-neighbor  nodes  were  avoided.  This  algorithm  has  a  time  complexity  of  O  {n) 
and  is  to  be  used  when  the  world  is  dynamic  and  n  is  very  large;  otherwise  the  All-Pairs 

Minimum  Cost  Paths  algorithm  with  time  complexity  O  (n) ,  is  far  better  because 

preprocessing  makes  the  path  class  finding  time  to  0(1). 

A  known  static  world  favors  the  use  of  pre-calculated  minimum  cost  paths  between 
all  pairs  of  nodes  in  the  respective  basic  connectivity  graph.  Thus,  tho,  All-Pairs  Minimum 
Cost  algorithm  is  ideally  suited  for  the  Yamabico  operating  environment.  For  this 
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environment,  the  time  complexity  Oin)  using  the  All-Pairs  Minimum  Cost  algorithm  is 

better  to  that  applying  the  Dijkstra  ’s  algorithm. 

The  latter  still  has  a  chance  when  the  graph  is  dense.  We  will  prefer  to  use  it,  to 
avoid  memory  space  problems  that  are  likely  to  occur  in  the  first  algorithm’s 
implementation.  Thus,  All-Pairs  Minimum  Cost  algorithm  is  the  optimal  method  for  global 
motion  planning. 
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APPENDIX  A.  USER  PROGRAMMS 


This  appendix  contains  the  program  files  which  describe  the  algorithms 
implemented  by  this  work.  Inputs  and  outputs  are  described  accordingly.  The  heading  of 
each  file  explains  the  details. 


77 


*  FE^E  :  dijkstra.C 

*  AUTHOR  :  Athanassios  Papadatos 

*  DATE  :  31  October  1995 

*  DESCRIPTION:  Minimum  Cost  Path  Finding.  Applies  to  Robot 

*  Global  Motion  Planning. 

*********:|:!l:**H:*******>|c**********************H:*M:*5t:**!f:*************/ 


Mnclude  <iostream.h> 

#include  <iomanip.h> 

Mnclude  <fstream.h> 

Mnclude  <math.h> 

Mnclude  <stdlib.h> 

Mefine  INFINITY  99 

#define  numOjNodes  22  //HERE  #  OF  NODES 
#define  out_file  “dijkstra.dat” 


ofstream  kds; 


//odjArray  is  an  array  of  pointers  to  nodes. 

//Each  pointer  ( odjArray [i]  )  points  to  the  address 
//of  node  GNodesfi],  to  be  initialized  when 
//build  the  Graph. 

struct  Node  { 
int  id; 
int  cost; 

int  mark;  //  its  value  is  “0”  or  “1  ” 

Node  *  prev;  //a  pointer  to  previous  node 

Node  *  odjArray [numOfNodes]; 

}; 


//a  Graph  is  an  array  of  nodes  &  an  array  of  edges  represented  by  their  cost. 
//Start  -  Goal  node  are  included  in  CGraph. 

//an  Edge  is  represented  by  [i][jj  the  GNodes,  and  by  its  cost. 

struct  Graph  { 

Node  GNodesjnumOfNodesJ; 

int  ECostj numOfNodes ][ numOfNodes]; 

}; 
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/♦*****************!|:**>|:****:|:*****:)c*****:i:****=l=******H=****=|: 

*  FUNCTION:  extractMin( Graph  G) 

*  DES  CRIPTION:  Searches  for  the  node  in  GNodesf ]  with  min  cost 

*  an  Returns  its  index. 


int 

extractMin(Graph  G) 

f 


Node  tmp; 

tmp  =  G.GNodes[numOfNodes-l];  //if  nothing  the  last  one  is  the  min!!!! 


int  min=  numOfNodes-1;  //at  begining  last  element  is  min  OR... 
for  (intj=0;  j<numOfNodes;  j++)  { 

if  (G.GNodesfjJ.mark  ==0)  { 

if  ( G.GNodes[j].cost  <  tmp.cost )  f 
tmp  =  G.GNodesfj]; 
min=j; 

} 

} 

} 

return  (min); 


} 


* 


*  FUNCTION  :Dijkstra() 

*  DESCRIPTION :  Applies  the  main  Dijkstra  ’5  algorithm 

* 


H:H:*********!l:****************M:********!|:*!|!j|!******H:M:*si.:j::Cs|.*:(;:C**:y 


void 

Dijkstra(  ) 

{ 

Graph  G; 

ifstream  injile(“ graph22.dat”,  ios::in); 
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> 


if(!in_file)  { 

cerr  «  “File  could  not  be  opened”  «  endl; 
exit(l); 

}. 


y*t*********************************************************** 

*  COMMENTS:  In  this  part  for  each  node  record  we  initialize  all  its  fields. 

*  that  is:  cost,  mark,  *prev. 

^it^it***********************************************************/ 


//Initialize  all  nodes. 

for  (intn=0;  n<numOfNodes;  n++)  { 


G.GNodes[n].id  =  n;  //if  the  first  node  then  cost  is  0 
G.GNodes[nJ.cost  =  INFINITY;  //n  ? INFINITY:  0; 
G.GNodes[n].mark  =  0; 

G.GNodes[n].prev  =  0; 


} 

kds«endl; 


/**************************************************************** 

*  COMMENTS :  In  this  part  of  the  code  we  initialize  Edge  Costs. 

*  It  initializes  a  2-diamensional  array  to  G.ECost[i][j]=  cost 

*  thus  for  the  edge  comprised  by  the  ith-jth  Nodes  we  ’ll  have  its  cost. 

He****************************************************************/ 

//When  looking  for  O’s  neighbors  it  has  one  if  there  is  edge  01. 

//When  looking  for  1  ’s  neighbors  it  has  one  if  there  is  edge  10. 


for(inti=0;  i<numOfNodes;  i++)  { 

for(int  j=0;  j<numOfNodes;  j++)  { 

int  cost; 

in_file  »  cost; 
G.ECost[iJ[jJ=  cost; 

} 

} 
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*COMMENTS :  We  Initialize  ADJ.  ARRAYS 

^de,  one  array  of  pointers  to  its  neighbors 

for  ( int  b=0;  b<numOfNodes;  b-\-+){ 
int  w=0; 

for  (int  c=0;  c<numOfNodes;  C++)  { 
if  ( G.ECost[b][c]  !=  INFINITY )  { 

G.GNodes[b].adjArray[w]  =  &  G.GNodesfc]; 

W++; 


} 

for  (int  d=M>+I;  d<numOfNodes;  d++ )  { 
G.GNodes[b].adjArray[d]  =  NULL; 


} 


} 


} 


//Here  I  assume  I’ve  already  build  the  Graph  with  Start  node  =  #0 
//and  goal  =  #(numOfNodes-I ) 

//All  the  nodes  have  cost=  9. 

//  mark,  *prev,  adj Arrays  initialized  by  run  time  when 
// I’ll  define  the  edges. 

//Let’s  begin  the  DIJKSTRA. 

//this  is  the  start  node 
G.  GNodes[ 0 J.cost=0; 


int  s=0; 

//find  neighbors 
do  { 


kds  <<  “\n  Min  cost  node 
kds  « 


MARKIT!!”«endl; 
«endl; 


G.GNodes[sJ.mark=  1; 

//pick  its  odjArray  of  neighbors  and for  each  neighbor... 


int  j=0; 

while  (G.GNodes[s].adjArray[j])  { //I  reduce  the  time 
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int  ind; 

ind  =  (*G.GNodes[s].adjArray[j]).id;  //the  id  of  neighbor 


intedgeCost  =  G.ECost[s][ind]; 

kds  «  “Vi ”«  s«“‘s neighbor  is:  “«  ind  «endl; 
kds«“ - "«endl; 

if(G.GNodes[s].cost  +  edgeCost  <  (*G.GNodes[s].adjArray[j]).cost )  { 
(*G.GNodes[s].adjArray[j]).cost  =  G.GNodes[s].cost  +  edgeCost; 

( *G.  GNodes[s].adjArray[j]).prev=  &G.  GNodes[sJ; 

kds  «  “  Dijkstra’s  step  DONE...”  «endl; 

kds  «  ind  « ‘“s  cost  =  “  «G.GNodes[ind].cost«endl; 

kds  «  ind  «“‘s  prev  — >  “  «  (*G.GNodes[ind].prev).id 

«endl; 


} 

else  { 

kds  «  “  Dijkstra’s  step  DONE...  ”  «endl; 

kds  «  ind  «  “  has  a  smaller  cost  already!!!  “  «endl; 

} 

;■++; 

///finish  with  this  “s”  node’s  neighbors 


kds  «endl«endl; 


//Pick  Minimum  Cost  Node 
s  =  extractMin(  G);  //new  neighbor 


}  while  (  G.GNodes[s].id  !=  numOfNodes-1 );  //GOAL  NODE  IS  numOfNodes-1 

//untill  goal  is  reached  ASSUME  GOAL 
//is  the  last  node  G.GNodes[numOfNodes-I].id 


G.GNodes[numOfNodes-l ].mark=  I;  //MARK  GOAL  node 


kds  <  <  “ViVi  Min  no Jg  15 

kds  «‘\iCost  of  Shortest  Path  =”«  G.GNodes[numOfNodes-l].cost  «endl; 
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kds  «  “\n\n  Shortest  Path  is: 

Node  *  tmp; 

tmp  =  &  G.GNodes[numOfNodes-lJ;  //Initialy,  tmp  points  to  the  GOAL  node 

kds  «  (*tmp).id; 

while  ( (*tmp).prev  !=0 )  ( 
kds  «"  —> 

kds  « (*(*tmp).prev).id; 

tmp  =  (*tmp).prev; 


} 


kds  «endl  «endl; 
}  //END  Dijkstra(G) 


* 


*  Function:  main( ) 

description:  Makes  the  necessary  calls  for  Dijkstra’s  algorithm 

*  to  execute. 


int 

main() 

{ 

kds.  open(  outjile ); 

DijkstraO; 

kds.closeO; 

retum(O); 


} 
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* 


*FILE  :  ap.C 

*  AUTHOR  :  Athanassios  Papadatos 

*DATE  :  1  Dec.  1995 

*  DESCRIPTION :  All-Pairs  Minimum  Cost  Paths  Algorithm 

*  ;  Applies  to  Robot  Global 

Motion  Planning. 

* 


Mnclude  <iostream.h> 

Mnclude  <iomanip.h> 

Mnclude  <fstream.h> 

Mnclude  <math.h> 

Mnclude  <stdlib.h> 

Mefine  INFINITY  99 

#define  numOfNodes  28  //HERE  #  OF  NODES 

#define  out_file  “ap.dat” 


of  stream  kds; 

struct  Node  { 
int  id; 


}; 


//a  Graph  is  an  array  of  nodes  &  an  array  of  edges 
//represented  by  their  cost. 

//Start  -  Goal  node  are  included  in  CGraph. 
struct  Graph  { 

Node  Nodes[numOfNodes]; 

int  Cost[ numOfNodesIlnumOfNodesI; 

//an  Edge  is  represented  by  [i][jl  the  GNodes, 
//and  by  its  cost. 

int  Previous! numOfN odes ][ numOfNodesI; 


//Input  X,  y  =  1.. numOfNodes 
void 

traceSPath(Graph  G,  intx,  inty) 

{ 
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kds<  <  “\n\n\n  Shortest  Path  “«y<  <“<- ”«x«  ”<  <endl; 

kds«“ - ”«endl; 

kds«y; 

x=x-l; 
y=y-l; 
if(  x!=y)  { 
while  (1){ 
int  tmp; 

//Previous  returns  node.id=l,..,n. 
tmp=G.Previous[xJ[yJ; 

Ms  «“<-”«tmp; 


y=(tmp-l); 


if( x==y ) 
break; 


} 

} 


} 


void 

allPairsO 

{ 

Graph  G; 


ifstream  in_file(“ graph28.dat”,  iosr.in); 
if(!in_file)  { 

cerr  «  “File  could  not  be  opened”  «  endl; 
exit(l); 

} 


//  INITIALIZE  NODES 


for(intn=0;  n<numOfNodes;  n++)  { 
G.NodesfnJ.id  =n+I; 


} 
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for(inti-0;  i<numOfNodes;  i++)  { 

for(intj=0;  j<numOfNodes;  j++)  { 

int  weight; 
injile  »  weight; 

G.  Cost[ i][j]=  weight; 


if  ( (weight  ==  INFINITY)  II  ( i  ==j  ) )( 

G.Previous[iJ[j]  =  0; 

//The  above  0  stands  for  NIL  predecessor. 

} 

else 

G.Previous[i][j]  =  G.NodesfiJ.id; 

} 

} 


{ 

kds«  “***  “«setiosflags(ios::left)«setw(3)« “1  ”«setw(3)« “2” 

«setw(3)«“3”«setw(3)«“4”«setw(3)«“5”«setw(3)«“6”«setw(3)« 
“7”«setw(3)«  “8”«setw(3)«  “9”«setw(3)«  “I0”«setw(3)«  “II  ”«setw 
(3)«“  12”  «setw(3)«“  13”  «setw(3)«“  14”  «setw(3)«“  15”  «setw(3)«“  1 
6” 

«setw(3)«“17”«setw(3)«“18”«setw(3)«“19”«setw(3)«“20”«setw(3 
)«“2I  ”«setw(3)«“22  ”«setw(3)«  “23”«setw(3)«  “24”  «setw(3)«  “25” 
«setw(3)«“26”«setw(3)«“27”«setw(3)«“28”«endl; 

H:  :!c  **  **********  **  ^  • 

> 

for  (int  a=0;  a<numOfNodes;  a++)/ 
kds«endl; 

kds«setiosflags( ios::lefi)«setw(2)«a+l«  “*  “; 
for  (inty=0;  y<numOfNodes;  y-^+){ 

kds«setiosflags(  ios::left)«setw(  3)«G.  Cost[ a][y]; 


} 

} 
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kds«endl<  <endl; 


kds<< “***  “ «setiosflags(ios::left)«setw(3)«“  1  ”«setw(3)« “2” 

«setw(3)«‘‘3”«setw(3)«“4”«setw(3)«“5”«setw(3)«“6”«setw(3)« 
“7”«setw(3)«  ’‘8”«setw(3)«  “9”«setw(3)«  “10”«setw(3)«“ll  ”«setw 
(3)«"12’>«setw(3)«‘‘13”«setw(3)«^‘14”«setw(3)«“15”«setw(3)«“l 
5” 

«setw(3)«“17”«setw(3)«“18”«setw(3)«“19”«setw(3)«“20”«setw(3 
)«  “21  ”«setw(3)«  “22”«setw(3)«  “23”«setw(3)«“24’'«setw(3)«  “25” 
«setw(3)«  “26”«setw{3)«  “27”«setw(3)«“28”«endl; 

]cds«  “*************************************************************** 


for  ( int  c=0;  c<numOfNodes;  c++){ 
kds«endl; 

kds«setiosflags(ios::left)«setw(2)«c+l«  “*  “; 
for  ( int  d=0;  d<numOfNodes;  d+  + ){ 

kds«setiosflags(  ios::lefi)«setw(  3)<  <G.Previous[c ][d ]; 


} 


} 


} 


kds<  <endl«endl; 

y:}:Hc:f:*5f:**5ic*******5i!*5|i5|«*5f:****:!:H;**************2};:iJ*********i*:ii:*:i:*?}:*****/ 


//Change  Costs  -  lengths  of  shortest  paths  -  and 
//  Previous 

for  ( int  m=0;  m<numOfNodes;  m+  +)  f 
for  ( intx=0;  x<numOfNodes;  x++ )  { 

for  ( int  y=0;  y<numOJNodes;  y++)  { 

if(  G.Cost[x][m]  +  G.Cost[m][y]  <  G.Cost[x][y] )  { 
if(G.Cost[x][m]!=INFINITY  &&  G.Cost[m] [y]! ^INFINITY )  { 

G.Cost[x][y]  =  G.Cost[xJ[m]  +  G.Cost[m][y]; 

G.Previous[x][y]  =  G.Previous[m][y]; 

} 

} 

else  { 


87 


G.Cost[x][y]  =  G.Cost[x][yJ; 
G.Previous[x][y]  =  G.Previous[xJ[y]; 


} 

} 


kds«endl«endl«endl«endl; 


} 


kds«“***  '‘«setiosflags(ios::left)«setw(3)« “1  ”«setw(3)« “2” 

<  <setw( 3)«“3  ”«setw(  3)«“4”<  <setw( 3)«“5”  «setw(  3)«“6”«setw(3)« 
“7”  «setw(3)«“8”  «setw(3)«“9”«setw(3)«“  10”  «setw(3)«“  11  ”«setw 
(3}«  "12  ”«setw(3)«  "13”«setw(3)«  "14”«setw(3)«"15”«setw(3)«  "1 
6” 

«setw(3)«  "17”«setw(3)«"18”«setw(3)«  ‘‘19”«setw(3)«  "20”«setw(3 
)«  "21  ”«setw(3)«  "22  ”«setw(3)«  "23  ”«setw(3)«  "24”  «setw(3)«"25” 
«setw(3)«  "26”«setw(3)«“27”«setw(3)«  "28”«endl; 

f 

for(int  a=0;  a<numOfNodes;  a+-¥)( 
kds«endl; 

kds«setiosflags(ios::lefi)«setw(2)«a+l« "* 
for  ( inty=0;  y<numOJNodes;  y++ )( 

kds<  <setiosflags(  ios::left)<  <setw(  3)«G.  Cost[ a  ][yj; 


} 


} 


kds«endl<  <endl; 


kds«  “***  “ «setiosflags( ios::left)«setw( 3)« "1  ”«setw(3)«  “2 ” 
«setw(3)«  "3”«setw(3)«  "4”«setw(3)«  "5”«setw(3)«  "6”«setw(3)« 
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'’7”«setw(3)«  “8'’«setw(3)«  "9”«setw(3)«  “10”«setw(3}«“ll  ”«setw 
(^)<<  “12”«setw(3)«  “13”«setw(3)«  “14”«setw(3)«“15”«setw(3)«  “1 


«setw(3)«  “17”«setw(3)«“18”«setw(3)«  ‘‘19”«setw(3)«  “20”«setw(3 
)«^‘21  ”«setw(3)«  “22"«setw(3)«  “23’’«setw(3)«  “24”«setw(3)«  “25’’ 
«setw(3)«  “26”«setw(3)«“27”«setw(3)«  “28”«endl; 

kds«"  *************************************************************** 

> 

for(intc=0;  c<numOfNodes;  c+-¥){ 
kds«endl; 

kds«setiosflags(ios::lefi)«setw(2)«c+l«  "* 
for  ( int  d=0;  d<numOfNodes;  d++){ 

kds<  <setiosflags(ios:  :left)<  <setw(  3)«G.Previous[  c J[dJ; 


} 


I 


traceSPath(G,  1,  7); 
traceSPath(  G,  1, 11 ); 
traceSPath(G,  1, 10); 
traceSPath(G,  1,  27); 

kds<  <endl«endl; 

} 


^  5j;  sj;  5}.  ^  5j;  5j;  ^  5jj  ^  ^  5j.  ^  ^  jJj  jfj  Jjj  ^  Jj.  jjj  ^  Jjj  ^  ^  Jj.  Jj.  ^ 

*FUNCT10N  :  mam() 

*  DESCRIPTION :  Makes  the  call  to  function  allPairs() 

*  and  arranges  to  open  and  close  the 

*  appropriate  files. 

********************  5l:**5(:****:ii5i:H;5j;**5N*5(:********H:/ 


int 

main() 

{ 


kds.  open(  out_file ); 


allPairsi); 

kds.closeO; 

retum(0); 


} 
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APPENDIX  B.  PROGRAMMS’  RESULTS 


This  appendix  contains  the  output  data  files  resulting  from  the  implementation  of 
algorithms  described  in  Appendix  A.  The  heading  of  each  file  explains  the  details. 
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*  FILE  :  dijkstra.dat 

*  AUTHOR  :  Athanassios  Papadatos 

*DATE  :  31  October  1995 

*  DESCRIPTION :  Contains  the  results  of  the  search  for  the  minimum  cost 


*  path  for  the  graph  in  Chapter  8 . 

jjc  jjC  ^  5jc  Sjc  5^  ^  Sjc  JjC  5jC  ^  ^  jjC  3^  5jC  3|C  SfC  ^  3jC  5jC  3jC  ^  3|C  5jC  jjC  5jC  jjC  ^  ^  SjC  SjC  3|i  ^  3fC  ^  5jC  jjC  3jC  ^  jjC  ^  ^|C  5jC  jJC  ^  ^  ^  j 


Min  cost  node  :  0,  MARK  IT! ! 

iiiiiifffiffiifiififimtfiiiiintifiiffffin 

O’s  neighbor  is:  5 


Dijkstra’s  step  DONE... 
5’s  cost  =  5 
5’s  prev  -— >  0 

O’s  neighbor  is:  14 


Dijkstra’s  step  DONE... 
14’s  cost  =  8 
14’s  prev  — >  0 


Min  cost  node  :  5,  MARK  IT!  I 
5’s  neighbor  is:  0 


Dijkstra’s  step  DONE... 

0  has  a  smaller  cost  already ! ! ! 

5’s  neighbor  is:  1 


Dijkstra’s  step  DONE... 
I’s  cost=  8 
I’s  prev  — >  5 

5’s  neighbor  is:  14 


Dijkstra’s  step  DONE... 

14  has  a  smaller  cost  already! ! ! 


Min  cost  node  :  1,  MARK  IT! ! 
////////////////////////////////////////////////// //////#fOT 
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Ts  neighbor  is:  2 


Dijkstra’s  step  DONE... 
2’s  cost=  10 
2’s  prev  — >  1 

Ts  neighbor  is:  5 


Dijkstra’s  step  DONE... 

5  has  a  smaller  cost  already! ! ! 


Min  cost  node  :  14,  MARK  IT! ! 
###//j//////////////////////////////////////////////////////^ 

14’s  neighbor  is:  0 


Dijkstra’s  step  DONE... 

0  has  a  smaller  cost  already! ! ! 

14’s  neighbor  is:  5 


Dijkstra’s  step  DONE... 

5  has  a  smaller  cost  already! ! ! 

14’s  neighbor  is:  17 


Dijkstra’s  step  DONE... 
17’s  cost=  11 
17’s  prev  — >  14 


Min  cost  node  :  2,  MARK  IT! ! 
2’s  neighbor  is:  1 


Dijkstra’s  step  DONE... 

1  has  a  smaller  cost  already! ! ! 

2’s  neighbor  is:  3 


Dijkstra’s  step  DONE... 
3’s  cost  =  12 
3’s  prev  — >  2 

2’s  neighbor  is:  6 


Dijkstra’s  step  DONE... 
6’s  cost=  13 
6’s  prev  >  2 


Min  cost  node  :  17,  MARK  IT!! 
17’s  neighbor  is:  14 
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Dijkstra’s  step  DONE... 

14  has  a  smaller  cost  already!  1 ! 

17’s  neighbor  is:  18 


Dijkstra’s  step  DONE... 
18’s  cost=  13 
18’s  prev  — >  17 


Min  cost  node  :  3,  MARK  IT! ! 
3’s  neighbor  is:  2 


Dijkstra’s  step  DONE... 

2  has  a  smaller  cost  already ! ! ! 

3’s  neighbor  is:  4 


Dijkstra’s  step  DONE... 
4’s  cost=  14 
4’s  prev  — >  3 

3’s  neighbor  is:  6 


Dijkstra’s  step  DONE... 

6  has  a  smaller  cost  already ! ! ! 


Min  cost  node  :  6,  MARK  IT! ! 
//////////////////tf#////////////////////////////////////////^ 

6’s  neighbor  is:  2 


Dijkstra’s  step  DONE... 

2  has  a  smaller  cost  already!!! 

6’s  neighbor  is:  3 


Dijkstra’s  step  DONE... 

3  has  a  smaller  cost  already! ! ! 

6’s  neighbor  is:  8 


Dijkstra’s  step  DONE... 
8’s  cost=  17 
8’s  prev  — >  6 


Min  cost  node  :  18,  MARK  IT!! 
II  If  If  If  11 II  If  If  fHfif  fill  fill  II  fill  fill  If  ^ 

18’s  neighbor  is:  15 


Dijkstra’s  step  DONE... 
15’s  cost  =  16 
15’s  prev  -— >  18 
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18’s  neighbor  is:  17 


Dijkstra’s  step  DONE... 

17  has  a  smaller  cost  already!! ! 

18’s  neighbor  is:  19 


Dijkstra’s  step  DONE... 
19’scost=  15 
19’s  prev  — >  18 


Min  cost  node  :  4,  MARK  IT! ' 

mi  II II II II II II II II II II II II II II II II II II II II II II II  nil. 


4’s  neighbor  is:  3 


Dijkstra’s  step  DONE... 

3  has  a  smaller  cost  already ! ! ! 

4’s  neighbor  is:  7 


Dijkstra’s  step  DONE... 
7’s  cost=  17 
7’s  prev  — >  4 


Min  cost  node  :  19,  MARK  IT' ' 

Ifilll  II II II II II II II II II II II II II II II II II II II II II II II II II II II II II 


19’s  neighbor  is:  15 


Dijkstra’s  step  DONE... 

15  has  a  smaller  cost  already ! ! ! 

19’s  neighbor  is:  18 


Dijkstra’s  step  DONE... 

18  has  a  smaller  cost  already! ! ! 

19’s  neighbor  is:  20 


Dijkstra’s  step  DONE... 
20’s  cost=  17 
20’s  prev  — >  19 


Min  cost  node  :  15,  MARK  IT" 
Ifllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll 


15’s  neighbor  is:  12 


Dijkstra’s  step  DONE... 
12’s  cost  =  20 
12’s  prev  -— >  15 

15’s  neighbor  is:  18 


95 


Dijkstra’s  step  DONE... 

18  has  a  smaller  cost  already! ! ! 

15’s  neighbor  is:  19 


Dijkstra’s  step  DONE... 

19  has  a  smaller  cost  already!! ! 


Min  cost  node  :  7,  MARK  IT!! 
7’s  neighbor  is:  4 


Dijkstra’s  step  DONE... 

4  has  a  smaller  cost  already !  1 ! 

7’s  neighbor  is:  9 


Dijkstra’s  step  DONE... 
9’s  cost  =  21 
9’s  prev  — >  7 

7’s  neighbor  is:  21 


Dijkstra’s  step  DONE... 
21’$  cost  =  21 
2rs  prev  — >  7 


Min  cost  node  :  8,  MARK  IT! ! 
8’s  neighbor  is:  6 


Dijkstra’s  step  DONE... 

6  has  a  smaller  cost  already! ! ! 

8’s  neighbor  is:  10 


Dijkstra’s  step  DONE... 
lO’s  cost  =  20 
lO’s  prev  — >  8 

8’s  neighbor  is:  12 


Dijkstra’s  step  DONE... 
12’s  cost  =  19 
12’s  prev  — >  8 


Min  cost  node  :  20,  MARK  IT! ! 
////////////////////////////////////////// ///N///////////W 

20’s  neighbor  is:  16 

Dijkstra’s  step  DONE... 
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16’s  cost  =  20 
16’s  prev  — >  20 

20’s  neighbor  is:  19 


Dijkstra’s  step  DONE... 

19  has  a  smaller  cost  already!! ! 


Min  cost  node  :  12,  MARK  IT! ! 


12’s  neighbor  is:  8 

Dijkstra’s  step  DONE... 

8  has  a  smaller  cost  already ! ! ! 

12’s  neighbor  is:  10 

Dijkstra’s  step  DONE... 

10  has  a  smaller  cost  already!! ! 

12’s  neighbor  is:  15 


Dijkstra’s  step  DONE... 

15  has  a  smaller  cost  already! ! ! 


Min  cost  node  :  10,  MARK  IT! ! 


lO’s  neighbor  is:  8 

Dijkstra’s  step  DONE... 

8  has  a  smaller  cost  already! ! ! 

lO’s  neighbor  is:  11 

Dijkstra’s  step  DONE... 
ll’s  cost  =  22 
11 ’s  prev  — >  10 

lO’s  neighbor  is:  12 

Dijkstra’s  step  DONE... 

12  has  a  smaller  cost  already!!! 


Min  cost  node  :  16,  MARK  IT!! 


16’s  neighbor  is:  13 


Dijkstra’s  step  DONE... 
13’s  cost  =  24 
13’s  prev  — >  16 

16’s  neighbor  is:  20 
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Dijkstra’s  step  DONE... 

20  has  a  smaller  cost  already!  1 ! 


Min  cost  node  is  GOAL.  Path  determined! ! ! 

Cost  of  Shortest  Path  =21 

Shortest  Path  is:  21  — >  7  — >  4  — >  3  — >  2  — >  1  — >  5  — >  0 
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* 


*  FILE  :  ap.dat 

*  AUTHOR  :  Athanassios  Papadatos 

=^DATE  :  31  October  1995 


*  DESCRIPTION :  Contains  the  result  output  for  All-Pairs  Minimum  Cost  Paths 

*  algorithm  for  the  graph  in  chapter  9. 
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7<— 5<— 3<— 1 
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IK— 3<— 1 
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